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Series  Editors’  Foreword 


The  topics  of  control  engineering  and  signal  processing  continue  to  flourish  and 
develop.  In  common  with  general  scientific  investigation,  new  ideas,  concepts 
and  interpretations  emerge  quite  spontaneously  and  these  are  then  discussed, 
used,  discarded  or  subsumed  into  the  prevailing  subject  paradigm.  Sometimes 
these  innovative  concepts  coalesce  into  a  new  sub-discipline  within  the  broad 
subject  tapestry  of  control  and  signal  processing.  This  preliminary  battle  be¬ 
tween  old  and  new  usually  takes  place  at  conferences,  through  the  Internet  and 
in  the  journals  of  the  discipline.  After  a  little  more  maturity  has  been  acquired 
by  the  new  concepts  then  archival  publication  as  a  scientific  or  engineering 
monograph  may  occur. 

A  new  concept  in  control  and  signal  processing  is  known  to  have  arrived 
when  sufficient  material  has  evolved  for  the  topic  to  be  taught  as  a  specialised 
tutorial  workshop  or  as  a  course  to  undergraduate,  graduate  or  industrial 
engineers.  Advanced  Textbooks  in  Control  and  Signal  Processing  are  designed 
as  a  vehicle  for  the  systematic  presentation  of  course  material  for  both  popular 
and  innovative  topics  in  the  discipline.  It  is  hoped  that  prospective  authors  will 
welcome  the  opportunity  to  publish  a  structured  and  systematic  presentation 
of  some  of  the  newer  emerging  control  and  signal  processing  technologies  in 
the  textbook  series. 

Robots  have  appeared  extensively  in  the  artistic  field  of  science  fiction 
writing.  The  actual  name  robot  arose  from  its  use  by  the  playwright  Karel 
Capek  in  the  play  Rossum’s  Universal  Robots  (1920).  Not  surprisingly,  the 
artistic  focus  has  been  on  mechanical  bipeds  with  anthropomorphic  person¬ 
alities  often  termed  androids.  This  focus  has  been  the  theme  of  such  cine¬ 
matic  productions  as,  I,  Robot  (based  on  Isaac  Asimov’s  stories)  and  Stanley 
Kubrick’s  film,  A. I.;  however,  this  book  demonstrates  that  robot  technology 
is  already  widely  used  in  industry  and  that  there  is  some  robot  technology 
which  is  at  prototype  stage  rapidly  approaching  introduction  to  commercial 
use.  Currently,  robots  may  be  classified  according  to  their  mobility  attributes 
as  shown  in  the  figure. 
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MOBILE  ROBOTS 


The  largest  class  of  robots  extant  today  is  that  of  the  fixed  robot  which 
does  repetitive  but  often  precise  mechanical  and  physical  tasks.  These  robots 
pervade  many  areas  of  modern  industrial  automation  and  are  mainly  con¬ 
cerned  with  tasks  performed  in  a  structured  environment.  It  seems  highly 
likely  that  as  the  technology  develops  the  number  of  mobile  robots  will  signif¬ 
icantly  increase  and  become  far  more  visible  as  more  applications  and  tasks 
in  an  unstructured  environment  are  serviced  by  robotic  technology. 

What  then  is  robotics?  A  succinct  definition  is  given  in  The  Chamber’s  Dic¬ 
tionary  (2003):  the  branch  of  technology  dealing  with  the  design,  construction 
and  use  of  robots.  This  definition  certainly  captures  the  spirit  of  this  volume 
in  the  Advanced  Textbooks  in  Control  and  Signal  Processing  series  entitled 
Robotics  and  written  by  Bruno  Siciliano,  Lorenzo  Sciavicco,  Luigi  Villani  and 
Giuseppe  Oriolo.  This  book  is  a  greatly  extended  and  revised  version  of  an 
earlier  book  in  the  series,  Modelling  and  Control  of  Robot  Manipulators  (2000, 
ISBN:  978-1-85233-221-1).  As  can  be  seen  from  the  figure  above,  robots  cover 
a  wide  variety  of  types  and  the  new  book  seeks  to  present  a  unified  approach 
to  robotics  whilst  focusing  on  the  two  leading  classes  of  robots,  the  fixed  and 
the  wheeled  types.  The  textbook  series  publishes  volumes  in  support  of  new 
disciplines  that  are  emerging  with  their  own  novel  identity,  and  robotics  as 
a  subject  certainly  falls  into  this  category.  The  full  scope  of  robotics  lies  at 
the  intersection  of  mechanics,  electronics,  signal  processing,  control  engineer¬ 
ing,  computing  and  mathematical  modelling.  However,  within  this  very  broad 
framework  the  authors  have  pursued  the  themes  of  modelling,  planning  and 
control.  These  are,  and  will  remain,  fundamental  aspects  of  robot  design  and 
operation  for  years  to  come.  Some  interesting  innovations  in  this  text  include 
material  on  wheeled  robots  and  on  vision  as  used  in  the  control  of  robots. 
Thus,  the  book  provides  a  thorough  theoretical  grounding  in  an  area  where 
the  technologies  are  evolving  and  developing  in  new  applications. 

The  series  is  one  of  textbooks  for  advanced  courses,  and  volumes  in  the 
series  have  useful  pedagogical  features.  This  volume  has  twelve  chapters  cov¬ 
ering  both  fundamental  and  specialist  topics,  and  there  is  a  Problems  section 
at  the  end  of  each  chapter.  Five  appendices  have  been  included  to  give  more 
depth  to  some  of  the  advanced  methods  used  in  the  text.  There  are  over  twelve 
pages  of  references  and  nine  pages  of  index.  The  details  of  the  citations  and 
index  should  also  facilitate  the  use  of  the  volume  as  a  source  of  reference  as 
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IX 


well  as  a  course  study  text.  We  expect  that  the  student,  the  researcher,  the 
lecturer  and  the  engineer  will  find  this  volume  of  great  value  for  the  study  of 
robotics. 


Glasgow  Michael  J.  Grimble 

August  2008  Michael  A.  Johnson 
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In  the  last  25  years,  the  field  of  robotics  has  stimulated  an  increasing  interest 
in  a  wide  number  of  scholars,  and  thus  literature  has  been  conspicuous,  both 
in  terms  of  textbooks  and  monographs,  and  in  terms  of  specialized  journals 
dedicated  to  robotics.  This  strong  interest  is  also  to  be  attributed  to  the  inter¬ 
disciplinary  character  of  robotics,  which  is  a  science  having  roots  in  different 
areas.  Cybernetics,  mechanics,  controls,  computers,  bioengineering,  electron¬ 
ics  —  to  mention  the  most  important  ones  —  are  all  cultural  domains  which 
undoubtedly  have  boosted  the  development  of  this  science. 

Despite  robotics  representing  as  yet  a  relatively  young  discipline,  its  foun¬ 
dations  are  to  be  considered  well-assessed  in  the  classical  textbook  literature. 
Among  these,  modelling,  planning  and  control  play  a  basic  role,  not  only  in  the 
traditional  context  of  industrial  robotics,  but  also  for  the  advanced  scenarios 
of  field  and  service  robots,  which  have  attracted  an  increasing  interest  from 
the  research  community  in  the  last  15  years. 

This  book  is  the  natural  evolution  of  the  previous  text  Modelling  and  Con¬ 
trol  of  Robot  Manipulators  by  the  first  two  co-authors,  published  in  1995,  and 
in  2000  with  its  second  edition.  The  cut  of  the  original  textbook  has  been 
confirmed  with  the  educational  goal  of  blending  the  fundamental  and  techno¬ 
logical  aspects  with  those  advanced  aspects,  on  a  uniform  track  as  regards  a 
rigorous  formalism. 

The  fundamental  and  technological  aspects  are  mainly  concentrated  in  the 
first  six  chapters  of  the  book  and  concern  the  theory  of  manipulator  structures, 
including  kinematics,  statics  and  trajectory  planning,  and  the  technology  of 
robot  actuators,  sensors  and  control  units. 

The  advanced  aspects  are  dealt  with  in  the  subsequent  six  chapters  and 
concern  dynamics  and  motion  control  of  robot  manipulators,  interaction  with 
the  environment  using  exteroceptive  sensory  data  (force  and  vision),  mobile 
robots  and  motion  planning. 

The  book  contents  are  organized  in  12  chapters  and  5  appendices. 

In  Chap.  1,  the  differences  between  industrial  and  advanced  applications 
are  enlightened  in  the  general  robotics  context .  The  most  common  mechanical 
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structures  of  robot  manipulators  and  wheeled  mobile  robots  are  presented. 
Topics  are  also  introduced  which  are  developed  in  the  subsequent  chapters. 

In  Chap.  2  kinematics  is  presented  with  a  systematic  and  general  approach 
which  refers  to  the  Denavit-Hartenberg  convention.  The  direct  kinematics 
equation  is  formulated  which  relates  joint  space  variables  to  operational  space 
variables.  This  equation  is  utilized  to  find  manipulator  workspace  as  well  as 
to  derive  a  kinematic  calibration  technique.  The  inverse  kinematics  problem 
is  also  analyzed  and  closed-form  solutions  are  found  for  typical  manipulation 
structures. 

Differential  kinematics  is  presented  in  Chap.  3.  The  relationship  between 
joint  velocities  and  end-effector  linear  and  angular  velocities  is  described  by 
the  geometric  Jacobian.  The  difference  between  the  geometric  Jacobian  and 
the  analytical  Jacobian  is  pointed  out.  The  Jacobian  constitutes  a  fundamen¬ 
tal  tool  to  characterize  a  manipulator,  since  it  allows  the  determination  of 
singular  configurations,  an  analysis  of  redundancy  and  the  expression  of  the 
relationship  between  forces  and  moments  applied  to  the  end-effector  and  the 
resulting  joint  torques  at  equilibrium  configurations  (statics).  Moreover,  the 
Jacobian  allows  the  formulation  of  inverse  kinematics  algorithms  that  solve 
the  inverse  kinematics  problem  even  for  manipulators  not  having  a  closed- form 
solution. 

In  Chap.  4,  trajectory  planning  techniques  are  illustrated  which  deal  with 
the  computation  of  interpolating  polynomials  through  a  sequence  of  desired 
points.  Both  the  case  of  point-to-point  motion  and  that  of  motion  through 
a  sequence  of  points  are  treated.  Techniques  are  developed  for  generating 
trajectories  both  in  the  joint  space  and  in  the  operational  space,  with  a  special 
concern  to  orientation  for  the  latter. 

Chapter  5  is  devoted  to  the  presentation  of  actuators  and  sensors.  After  an 
illustration  of  the  general  features  of  an  actuating  system,  methods  to  control 
electric  and  hydraulic  drives  are  presented.  The  most  common  proprioceptive 
and  exteroceptive  sensors  in  robotics  are  described. 

In  Chap.  6,  the  functional  architecture  of  a  robot  control  system  is  illus¬ 
trated.  The  characteristics  of  programming  environments  are  presented  with 
an  emphasis  on  teaching-by-showing  and  robot-oriented  programming.  A  gen¬ 
eral  model  for  the  hardware  architecture  of  an  industrial  robot  control  system 
is  finally  discussed. 

Chapter  7  deals  with  the  derivation  of  manipulator  dynamics,  which  plays 
a  fundamental  role  in  motion  simulation,  manipulation  structure  analysis  and 
control  algorithm  synthesis.  The  dynamic  model  is  obtained  by  explicitly  tak¬ 
ing  into  account  the  presence  of  actuators.  Two  approaches  are  considered, 
namely,  one  based  on  Lagrange  formulation,  and  the  other  based  on  Newton- 
Euler  formulation.  The  former  is  conceptually  simpler  and  systematic,  whereas 
the  latter  allows  computation  of  a  dynamic  model  in  a  recursive  form.  Notable 
properties  of  the  dynamic  model  are  presented,  including  linearity  in  the  pa¬ 
rameters  which  is  utilized  to  develop  a  model  identification  technique.  Finally, 
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the  transformations  needed  to  express  the  dynamic  model  in  the  operational 
space  are  illustrated. 

In  Chap.  8  the  problem  of  motion  control  in  free  space  is  treated.  The 
distinction  between  joint  space  decentralized  and  centralized  control  strategies 
is  pointed  out.  With  reference  to  the  former,  the  independent  joint  control 
technique  is  presented  which  is  typically  used  for  industrial  robot  control. 
As  a  premise  to  centralized  control,  the  computed  torque  feedforward  control 
technique  is  introduced.  Advanced  schemes  are  then  introduced  including  PD 
control  with  gravity  compensation,  inverse  dynamics  control,  robust  control, 
and  adaptive  control.  Centralized  techniques  are  extended  to  operational  space 
control. 

Force  control  of  a  manipulator  in  contact  with  the  working  environment 
is  tackled  in  Chap.  9.  The  concepts  of  mechanical  compliance  and  impedance 
are  defined  as  a  natural  extension  of  operational  space  control  schemes  to  the 
constrained  motion  case.  Force  control  schemes  are  then  presented  which  are 
obtained  by  the  addition  of  an  outer  force  feedback  loop  to  a  motion  control 
scheme.  The  hybrid  force/motion  control  strategy  is  finally  presented  with 
reference  to  the  formulation  of  natural  and  artificial  constraints  describing  an 
interaction  task. 

In  Chap.  10,  visual  control  is  introduced  which  allows  the  use  of  infor¬ 
mation  on  the  environment  surrounding  the  robotic  system.  The  problems  of 
camera  position  and  orientation  estimate  with  respect  to  the  objects  in  the 
scene  are  solved  by  resorting  to  both  analytical  and  numerical  techniques. 
After  presenting  the  advantages  to  be  gained  with  stereo  vision  and  a  suit¬ 
able  camera  calibration,  the  two  main  visual  control  strategies  are  illustrated, 
namely  in  the  operational  space  and  in  the  image  space,  whose  advantages  can 
be  effectively  combined  in  the  hybrid  visual  control  scheme. 

Wheeled  mobile  robots  are  dealt  with  in  Chap.  11,  which  extends  some 
modelling,  planning  and  control  aspects  of  the  previous  chapters.  As  far 
as  modelling  is  concerned,  it  is  worth  distinguishing  between  the  kinematic 
model,  strongly  characterized  by  the  type  of  constraint  imposed  by  wheel 
rolling,  and  the  dynamic  model  which  accounts  for  the  forces  acting  on  the 
robot.  The  peculiar  structure  of  the  kinematic  model  is  keenly  exploited  to 
develop  both  path  and  trajectory  planning  techniques.  The  control  problem 
is  tackled  with  reference  to  two  main  motion  tasks:  trajectory  tracking  and 
configuration  regulation.  Further,  it  is  evidenced  how  the  implementation  of 
the  control  schemes  utilizes  odometric  localization  methods. 

Chapter  12  reprises  the  planning  problems  treated  in  Chaps.  4  and  11 
for  robot  manipulators  and  mobile  robots  respectively,  in  the  case  when  ob¬ 
stacles  are  present  in  the  workspace.  In  this  framework,  motion  planning  is 
referred  to,  which  is  effectively  formulated  in  the  configuration  space.  Several 
planning  techniques  for  mobile  robots  are  then  presented:  retraction,  cell  de¬ 
composition,  probabilistic,  artificial  potential.  The  extension  to  the  case  of 
robot  manipulators  is  finally  discussed. 
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This  chapter  concludes  the  presentation  of  the  topical  contents  of  the  text¬ 
book;  five  appendices  follow  which  have  been  included  to  recall  background 
methodologies. 

Appendix  A  is  devoted  to  linear  algebra  and  presents  the  fundamental 
notions  on  matrices,  vectors  and  related  operations. 

Appendix  B  presents  those  basic  concepts  of  rigid  body  mechanics  which 
are  preliminary  to  the  study  of  manipulator  kinematics,  statics  and  dynamics. 

Appendix  C  illustrates  the  principles  of  feedback  control  of  linear  systems 
and  presents  a  general  method  based  on  Lyapunov  theory  for  control  of  non¬ 
linear  systems. 

Appendix  D  deals  with  some  concepts  of  differential  geometry  needed  for 
control  of  mechanical  systems  subject  to  nonholonomic  constraints. 

Appendix  E  is  focused  on  graph  search  algorithms  and  their  complexity  in 
view  of  application  to  motion  planning  methods. 

The  organization  of  the  contents  according  to  the  above  illustrated  scheme 
allows  the  adoption  of  the  book  as  a  reference  text  for  a  senior  undergrad¬ 
uate  or  graduate  course  in  automation,  computer,  electrical,  electronics,  or 
mechanical  engineering  with  strong  robotics  content. 

From  a  pedagogical  viewpoint,  the  various  topics  are  presented  in  an  in¬ 
strumental  manner  and  are  developed  with  a  gradually  increasing  level  of  diffi¬ 
culty.  Problems  are  raised  and  proper  tools  are  established  to  find  engineering- 
oriented  solutions.  Each  chapter  is  introduced  by  a  brief  preamble  providing 
the  rationale  and  the  objectives  of  the  subject  matter.  The  topics  needed  for  a 
proficient  study  of  the  text  are  presented  in  the  five  appendices,  whose  purpose 
is  to  provide  students  of  different  extraction  with  a  homogeneous  background. 

The  book  contains  more  than  310  illustrations  and  more  than  60  workcd- 
out  examples  and  case  studies  spread  throughout  the  text  with  frequent  resort 
to  simulation.  The  results  of  computer  implementations  of  inverse  kinemat¬ 
ics  algorithms,  trajectory  planning  techniques,  inverse  dynamics  computation, 
motion,  force  and  visual  control  algorithms  for  robot  manipulators,  and  mo¬ 
tion  control  for  mobile  robots  are  presented  in  considerable  detail  in  order  to 
facilitate  the  comprehension  of  the  theoretical  development,  as  well  as  to  in¬ 
crease  sensitivity  of  application  in  practical  problems.  In  addition,  nearly  150 
end-of-chapter  problems  are  proposed,  some  of  which  contain  further  study 
matter  of  the  contents,  and  the  book  is  accompanied  by  an  electronic  solu¬ 
tions  manual  (downloadable  from  www.springer.com/9T8-l-84628-641-4) 
containing  the  MATLAB®  code  for  computer  problems;  this  is  available  free 
of  charge  to  those  adopting  this  volume  as  a  text  for  courses.  Special  care  has 
been  devoted  to  the  selection  of  bibliographical  references  (more  than  250) 
which  are  cited  at  the  end  of  each  chapter  in  relation  to  the  historical  devel¬ 
opment  of  the  field. 
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Introduction 


Robotics  is  concerned  with  the  study  of  those  machines  that  can  replace  hu¬ 
man  beings  in  the  execution  of  a  task,  as  regards  both  physical  activity  and 
decision  making.  The  goal  of  the  introductory  chapter  is  to  point  out  the 
problems  related  to  the  use  of  robots  in  industrial  applications,  as  well  as  the 
perspectives  offered  by  advanced  robotics.  A  classification  of  the  most  common 
mechanical  structures  of  robot  manipulators  and  mobile  robots  is  presented. 
Topics  of  modelling ,  planning  and  control  are  introduced  which  will  be  ex¬ 
amined  in  the  following  chapters.  The  chapter  ends  with  a  list  of  references 
dealing  with  subjects  both  of  specific  interest  and  of  related  interest  to  those 
covered  by  this  textbook. 


1.1  Robotics 

Robotics  has  profound  cultural  roots.  Over  the  course  of  centuries,  human  be¬ 
ings  have  constantly  attempted  to  seek  substitutes  that  would  be  able  to  mimic 
their  behaviour  in  the  various  instances  of  interaction  with  the  surrounding 
environment.  Several  motivations  have  inspired  this  continuous  search  refer¬ 
ring  to  philosophical,  economic,  social  and  scientific  principles. 

One  of  human  beings’  greatest  ambitions  has  been  to  give  life  to  their 
artifacts.  The  legend  of  the  Titan  Prometheus,  who  molded  humankind  from 
clay,  as  well  as  that  of  the  giant  Talus,  the  bronze  slave  forged  by  Hephaestus, 
testify  how  Greek  mythology  was  influenced  by  that  ambition,  which  has  been 
revisited  in  the  tale  of  Frankenstein  in  modern  times. 

Just  as  the  giant  Talus  was  entrusted  with  the  task  of  protecting  the 
island  of  Crete  from  invaders,  in  the  Industrial  Age  a  mechanical  creature 
( automaton )  has  been  entrusted  with  the  task  of  substituting  a  human  being 
in  subordinate  labor  duties.  This  concept  was  introduced  by  the  Czech  play¬ 
wright  Karel  Capek  who  wrote  the  play  Rossum’s  Universal  Robots  (R.  U.R.) 
in  1920.  On  that  occasion  he  coined  the  term  robot  —  derived  from  the  term 
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robota  that  means  executive  labour  in  Slav  languages  —  to  denote  the  au¬ 
tomaton  built  by  Rossum  who  ends  up  by  rising  up  against  humankind  in  the 
science  fiction  tale. 

In  the  subsequent  years,  in  view  of  the  development  of  science  fiction,  the 
behaviour  conceived  for  the  robot  has  often  been  conditioned  by  feelings.  This 
has  contributed  to  rendering  the  robot  more  and  more  similar  to  its  creator. 

It  is  worth  noticing  how  Rossum’s  robots  were  represented  as  creatures 
made  with  organic  material.  The  image  of  the  robot  as  a  mechanical  artifact 
starts  in  the  1940s  when  the  Russian  Isaac  Asimov,  the  well-known  science 
fiction  writer,  conceived  the  robot  as  an  automaton  of  human  appearance  but 
devoid  of  feelings.  Its  behaviour  was  dictated  by  a  “positronic”  brain  pro¬ 
grammed  by  a  human  being  in  such  a  way  as  to  satisfy  certain  rules  of  ethical 
conduct.  The  term  robotics  was  then  introduced  by  Asimov  as  the  science 
devoted  to  the  study  of  robots  which  was  based  on  the  three  fundamental 
laws: 

1.  A  robot  may  not  injure  a  human  being  or,  through  inaction,  allow  a  human 
being  to  come  to  harm. 

2.  A  robot  must  obey  the  orders  given  by  human  beings,  except  when  such 
orders  would  conflict  with  the  first  law. 

3.  A  robot  must  protect  its  own  existence,  as  long  as  such  protection  does 
not  conflict  with  the  first  or  second  law. 

These  laws  established  rules  of  behaviour  to  consider  as  specifications  for 
the  design  of  a  robot,  which  since  then  has  attained  the  connotation  of  an 
industrial  product  designed  by  engineers  or  specialized  technicians. 

Science  fiction  has  influenced  the  man  and  the  woman  in  the  street  that 
continue  to  imagine  the  robot  as  a  humanoid  who  can  speak,  walk,  see,  and 
hear,  with  an  appearance  very  much  like  that  presented  by  the  robots  of  the 
movie  Metropolis ,  a  precursor  of  modern  cinematography  on  robots,  with  Star 
Wars  and  more  recently  with  I,  Robot  inspired  by  Asimov’s  novels. 

According  to  a  scientific  interpretation  of  the  science-fiction  scenario,  the 
robot  is  seen  as  a  machine  that,  independently  of  its  exterior,  is  able  to  modify 
the  environment  in  which  it  operates.  This  is  accomplished  by  carrying  out 
actions  that  are  conditioned  by  certain  rules  of  behaviour  intrinsic  in  the 
machine  as  well  as  by  some  data  the  robot  acquires  on  its  status  and  on  the 
environment.  In  fact,  robotics  is  commonly  defined  as  the  science  studying  the 
intelligent  connection  between  perception  and  action. 

With  reference  to  this  definition,  a  robotic  system  is  in  reality  a  complex 
system,  functionally  represented  by  multiple  subsystems  (Fig.  1.1). 

The  essential  component  of  a  robot  is  the  mechanical  system  endowed,  in 
general,  with  a  locomotion  apparatus  (wheels,  crawlers,  mechanical  legs)  and 
a  manipulation  apparatus  (mechanical  arms,  end-effectors,  artificial  hands). 
As  an  example,  the  mechanical  system  in  Fig.  1.1  consists  of  two  mechanical 
arms  (manipulation  apparatus),  each  of  which  is  carried  by  a  mobile  vehicle 
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Fig.  1.1.  Components  of  a  robotic  system 


(locomotion  apparatus).  The  realization  of  such  a  system  refers  to  the  context 
of  design  of  articulated  mechanical  systems  and  choice  of  materials. 

The  capability  to  exert  an  action,  both  locomotion  and  manipulation,  is 
provided  by  an  actuation  system  which  animates  the  mechanical  components 
of  the  robot.  The  concept  of  such  a  system  refers  to  the  context  of  motion 
control ,  dealing  with  servomotors,  drives  and  transmissions . 

The  capability  for  perception  is  entrusted  to  a  sensory  system  which  can 
acquire  data  on  the  internal  status  of  the  mechanical  system  ( proprioceptive 
sensors,  such  as  position  transducers)  as  well  as  on  the  external  status  of 
the  environment  ( exteroceptive  sensors,  such  as  force  sensors  and  cameras). 
The  realization  of  such  a  system  refers  to  the  context  of  materials  properties, 
signal  conditioning,  data  processing,  and  information  retrieval. 

The  capability  for  connecting  action  to  perception  in  an  intelligent  fash¬ 
ion  is  provided  by  a  control  system  which  can  command  the  execution  of  the 
action  in  respect  to  the  goals  set  by  a  task  planning  technique,  as  well  as 
of  the  constraints  imposed  by  the  robot  and  the  environment.  The  realiza¬ 
tion  of  such  a  system  follows  the  same  feedback  principle  devoted  to  control 
of  human  body  functions,  possibly  exploiting  the  description  of  the  robotic 
system’s  components  (modelling) .  The  context  is  that  of  cybernetics,  dealing 
with  control  and  supervision  of  robot  motions,  artificial  intelligence  and  expert 
systems,  the  computational  architecture  and  programming  environment. 

Therefore,  it  can  be  recognized  that  robotics  is  an  interdisciplinary  subject 
concerning  the  cultural  areas  of  mechanics,  control,  computers,  and  electron¬ 
ics. 


1.2  Robot  Mechanical  Structure 

The  key  feature  of  a  robot  is  its  mechanical  structure.  Robots  can  be  classified 
as  those  with  a  fixed  base,  robot  manipulators ,  and  those  with  a  mobile  base, 
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mobile  robots.  In  the  following,  the  geometrical  features  of  the  two  classes  are 
presented. 

1.2.1  Robot  Manipulators 

The  mechanical  structure  of  a  robot  manipulator  consists  of  a  sequence  of  rigid 
bodies  (links)  interconnected  by  means  of  articulations  (joints)'  a  manipulator 
is  characterized  by  an  arm  that  ensures  mobility,  a  wrist  that  confers  dexterity, 
and  an  end-effector  that  performs  the  task  required  of  the  robot. 

The  fundamental  structure  of  a  manipulator  is  the  serial  or  open  kinematic 
chain.  From  a  topological  viewpoint,  a  kinematic  chain  is  termed  open  when 
there  is  only  one  sequence  of  links  connecting  the  two  ends  of  the  chain.  Al¬ 
ternatively,  a  manipulator  contains  a  closed  kinematic  chain  when  a  sequence 
of  links  forms  a  loop. 

A  manipulator’s  mobility  is  ensured  by  the  presence  of  joints.  The  artic¬ 
ulation  between  two  consecutive  links  can  be  realized  by  means  of  either  a 
prismatic  or  a  revolute  joint.  In  an  open  kinematic  chain,  each  prismatic  or 
revolute  joint  provides  the  structure  with  a  single  degree  of  freedom  (DOF).  A 
prismatic  joint  creates  a  relative  translational  motion  between  the  two  links, 
whereas  a  revolute  joint  creates  a  relative  rotational  motion  between  the  two 
links.  Revolute  joints  are  usually  preferred  to  prismatic  joints  in  view  of  their 
compactness  and  reliability.  On  the  other  hand,  in  a  closed  kinematic  chain, 
the  number  of  DOFs  is  less  than  the  number  of  joints  in  view  of  the  constraints 
imposed  by  the  loop. 

The  degrees  of  freedom  should  be  properly  distributed  along  the  mechan¬ 
ical  structure  in  order  to  have  a  sufficient  number  to  execute  a  given  task. 
In  the  most  general  case  of  a  task  consisting  of  arbitrarily  positioning  and 
orienting  an  object  in  three-dimensional  (3D)  space,  six  DOFs  are  required, 
three  for  positioning  a  point  on  the  object  and  three  for  orienting  the  object 
with  respect  to  a  reference  coordinate  frame.  If  more  DOFs  than  task  vari¬ 
ables  are  available,  the  manipulator  is  said  to  be  redundant  from  a  kinematic 
viewpoint. 

The  workspace  represents  that  portion  of  the  environment  the  manipula¬ 
tor’s  end-effector  can  access.  Its  shape  and  volume  depend  on  the  manipulator 
structure  as  well  as  on  the  presence  of  mechanical  joint  limits. 

The  task  required  of  the  arm  is  to  position  the  wrist  which  then  is  required 
to  orient  the  end-effector.  The  type  and  sequence  of  the  arm’s  DOFs,  start¬ 
ing  from  the  base  joint,  allows  a  classification  of  manipulators  as  Cartesian, 
cylindrical,  spherical,  SCARA ,  and  anthropomorphic. 

Cartesian  geometry  is  realized  by  three  prismatic  joints  whose  axes  typ¬ 
ically  are  mutually  orthogonal  (Fig.  1.2).  In  view  of  the  simple  geometry, 
each  DOF  corresponds  to  a  Cartesian  space  variable  and  thus  it  is  natu¬ 
ral  to  perform  straight  motions  in  space.  The  Cartesian  structure  offers  very 
good  mechanical  stiffness.  Wrist  positioning  accuracy  is  constant  everywhere 
in  the  workspace.  This  is  the  volume  enclosed  by  a  rectangular  parallel-piped 
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(Fig.  1.2).  As  opposed  to  high  accuracy,  the  structure  has  low  dexterity  since 
all  the  joints  are  prismatic.  The  direction  of  approach  in  order  to  manipu¬ 
late  an  object  is  from  the  side.  On  the  other  hand,  if  it  is  desired  to  ap¬ 
proach  an  object  from  the  top,  the  Cartesian  manipulator  can  be  realized  by 
a  gantry  structure  as  illustrated  in  Fig.  1.3.  Such  a  structure  makes  available 
a  workspace  with  a  large  volume  and  enables  the  manipulation  of  objects  of 
large  dimensions  and  heavy  weight.  Cartesian  manipulators  are  employed  for 
material  handling  and  assembly.  The  motors  actuating  the  joints  of  a  Carte¬ 
sian  manipulator  are  typically  electric  and  occasionally  pneumatic. 

Cylindrical  geometry  differs  from  Cartesian  in  that  the  first  prismatic  joint 
is  replaced  with  a  revolute  joint  (Fig.  1.4).  If  the  task  is  described  in  cylindri- 
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Fig.  1.5.  Spherical  manipulator  and  its  workspace 


cal  coordinates,  in  this  case  each  DOF  also  corresponds  to  a  Cartesian  space 
variable.  The  cylindrical  structure  offers  good  mechanical  stiffness.  Wrist  posi¬ 
tioning  accuracy  decreases  as  the  horizontal  stroke  increases.  The  workspace  is 
a  portion  of  a  hollow  cylinder  (Fig.  1.4).  The  horizontal  prismatic  joint  makes 
the  wrist  of  a  cylindrical  manipulator  suitable  to  access  horizontal  cavities. 
Cylindrical  manipulators  are  mainly  employed  for  carrying  objects  even  of 
large  dimensions;  in  such  a  case  the  use  of  hydraulic  motors  is  to  be  preferred 
to  that  of  electric  motors. 

Spherical  geometry  differs  from  cylindrical  in  that  the  second  prismatic 
joint  is  replaced  with  a  revolute  joint  (Fig.  1.5).  Each  DOF  corresponds  to  a 
Cartesian  space  variable  provided  that  the  task  is  described  in  spherical  coor¬ 
dinates.  Mechanical  stiffness  is  lower  than  the  above  two  geometries  and  me¬ 
chanical  construction  is  more  complex.  Wrist  positioning  accuracy  decreases 
as  the  radial  stroke  increases.  The  workspace  is  a  portion  of  a  hollow  sphere 
(Fig.  1.5);  it  can  also  include  the  supporting  base  of  the  manipulator  and  thus 


1.2  Robot  Mechanical  Structure 


7 


Fig.  1.6.  SCARA  manipulator  and  its  workspace 


Fig.  1.7.  Anthropomorphic  manipulator  and  its  workspace 

it  can  allow  manipulation  of  objects  on  the  floor.  Spherical  manipulators  are 
mainly  employed  for  machining.  Electric  motors  are  typically  used  to  actuate 
the  joints. 

A  special  geometry  is  SCARA  geometry  that  can  be  realized  by  disposing 
two  revolute  joints  and  one  prismatic  joint  in  such  a  way  that  all  the  axes 
of  motion  are  parallel  (Fig.  1.6).  The  acronym  SCARA  stands  for  Selective 
Compliance  Assembly  Robot  Arm  and  characterizes  the  mechanical  features 
of  a  structure  offering  high  stiffness  to  vertical  loads  and  compliance  to  hori¬ 
zontal  loads.  As  such,  the  SCARA  structure  is  well-suited  to  vertical  assembly 
tasks.  The  correspondence  between  the  DOFs  and  Cartesian  space  variables 
is  maintained  only  for  the  vertical  component  of  a  task  described  in  Carte¬ 
sian  coordinates.  Wrist  positioning  accuracy  decreases  as  the  distance  of  the 
wrist  from  the  first  joint  axis  increases.  The  typical  workspace  is  illustrated 
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Fig.  1.9.  Parallel  manipulator 


in  Fig.  1.6.  The  SCARA  manipulator  is  suitable  for  manipulation  of  small 
objects;  joints  are  actuated  by  electric  motors. 

Anthropomorphic  geometry  is  realized  by  three  revolute  joints;  the  revolute 
axis  of  the  first  joint  is  orthogonal  to  the  axes  of  the  other  two  which  are 
parallel  (Fig.  1.7).  By  virtue  of  its  similarity  with  the  human  arm,  the  second 
joint  is  called  the  shoulder  joint  and  the  third  joint  the  elbow  joint  since 
it  connects  the  “arm”  with  the  “forearm.”  The  anthropomorphic  structure 
is  the  most  dexterous  one,  since  all  the  joints  are  revolute.  On  the  other 
hand,  the  correspondence  between  the  DOFs  and  the  Cartesian  space  variables 
is  lost,  and  wrist  positioning  accuracy  varies  inside  the  workspace.  This  is 
approximately  a  portion  of  a  sphere  (Fig.  1.7)  and  its  volume  is  large  compared 
to  manipulator  encumbrance.  Joints  are  typically  actuated  by  electric  motors. 
The  range  of  industrial  applications  of  anthropomorphic  manipulators  is  wide. 
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Fig.  1.10.  Hybrid  parallel-serial  manipulator 


According  to  the  latest  report  by  the  International  Federation  of  Robotics 
(IFR),  up  to  2005,  59%  of  installed  robot  manipulators  worldwide  has  an¬ 
thropomorphic  geometry,  20%  has  Cartesian  geometry,  12%  has  cylindrical 
geometry,  and  8%  has  SCARA  geometry. 

All  the  previous  manipulators  have  an  open  kinematic  chain.  Whenever 
larger  payloads  are  required,  the  mechanical  structure  will  have  higher  stiffness 
to  guarantee  comparable  positioning  accuracy.  In  such  a  case,  resorting  to 
a  closed  kinematic  chain  is  advised.  For  instance,  for  an  anthropomorphic 
structure,  parallelogram  geometry  between  the  shoulder  and  elbow  joints  can 
be  adopted,  so  as  to  create  a  closed  kinematic  chain  (Fig.  1.8). 

An  interesting  closed-chain  geometry  is  parallel  geometry  (Fig.  1.9)  which 
has  multiple  kinematic  chains  connecting  the  base  to  the  end-effector.  The 
fundamental  advantage  is  seen  in  the  high  structural  stiffness,  with  respect  to 
open-chain  manipulators,  and  thus  the  possibility  to  achieve  high  operational 
speeds;  the  drawback  is  that  of  having  a  reduced  workspace. 

The  geometry  illustrated  in  Fig.  1.10  is  of  hybrid  type,  since  it  consists 
of  a  parallel  arm  and  a  serial  kinematic  chain.  This  structure  is  suitable  for 
the  execution  of  manipulation  tasks  requiring  large  values  of  force  along  the 
vertical  direction. 

The  manipulator  structures  presented  above  are  required  to  position  the 
wrist  which  is  then  required  to  orient  the  manipulator’s  end-effector.  If  arbi¬ 
trary  orientation  in  3D  space  is  desired,  the  wrist  must  possess  at  least  three 
DOFs  provided  by  revolute  joints.  Since  the  wrist  constitutes  the  terminal 
part  of  the  manipulator,  it  has  to  be  compact;  this  often  complicates  its  me¬ 
chanical  design.  Without  entering  into  construction  details,  the  realization 
endowing  the  wrist  with  the  highest  dexterity  is  one  where  the  three  revolute 
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Fig.  1.11.  Spherical  wrist 

axes  intersect  at  a  single  point.  In  such  a  case,  the  wrist  is  called  a  spherical 
wrist ,  as  represented  in  Fig.  1.11.  The  key  feature  of  a  spherical  wrist  is  the 
decoupling  between  position  and  orientation  of  the  end-effector;  the  arm  is  en¬ 
trusted  with  the  task  of  positioning  the  above  point  of  intersection,  whereas 
the  wrist  determines  the  end-effector  orientation.  Those  realizations  where  the 
wrist  is  not  spherical  are  simpler  from  a  mechanical  viewpoint,  but  position 
and  orientation  are  coupled,  and  this  complicates  the  coordination  between 
the  motion  of  the  arm  and  that  of  the  wrist  to  perform  a  given  task. 

The  end- effector  is  specified  according  to  the  task  the  robot  should  ex¬ 
ecute.  For  material  handling  tasks,  the  end-effector  consists  of  a  gripper 
of  proper  shape  and  dimensions  determined  by  the  object  to  be  grasped 
(Fig.  1.11).  For  machining  and  assembly  tasks,  the  end-effector  is  a  tool  or 
a  specialized  device,  e.g.,  a  welding  torch,  a  spray  gun,  a  mill,  a  drill,  or  a 
screwdriver. 

The  versatility  and  flexibility  of  a  robot  manipulator  should  not  induce 
the  conviction  that  all  mechanical  structures  are  equivalent  for  the  execution 
of  a  given  task.  The  choice  of  a  robot  is  indeed  conditioned  by  the  application 
which  sets  constraints  on  the  workspace  dimensions  and  shape,  the  maximum 
payload,  positioning  accuracy,  and  dynamic  performance  of  the  manipulator. 

1.2.2  Mobile  Robots 

The  main  feature  of  mobile  robots  is  the  presence  of  a  mobile  base  which 
allows  the  robot  to  move  freely  in  the  environment.  Unlike  manipulators,  such 
robots  are  mostly  used  in  service  applications,  where  extensive,  autonomous 
motion  capabilities  are  required.  From  a  mechanical  viewpoint,  a  mobile  robot 
consists  of  one  or  more  rigid  bodies  equipped  with  a  locomotion  system.  This 
description  includes  the  following  two  main  classes  of  mobile  robots:1 

•  Wheeled  mobile  robots  typically  consist  of  a  rigid  body  ( base  or  chassis) 
and  a  system  of  wheels  which  provide  motion  with  respect  to  the  ground. 

1  Other  types  of  mechanical  locomotion  systems  are  not  considered  here.  Among 
these,  it  is  worth  mentioning  tracked  locomotion ,  very  effective  on  uneven  terrain, 
and  undulatory  locomotion,  inspired  by  snake  gaits,  which  can  be  achieved  with¬ 
out  specific  devices.  There  also  exist  types  of  locomotion  that  are  not  constrained 
to  the  ground,  such  as  flying  and  navigation. 
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Other  rigid  bodies  (trailers),  also  equipped  with  wheels,  may  be  connected 
to  the  base  by  means  of  revolute  joints. 

•  Legged  mobile  robots  are  made  of  multiple  rigid  bodies,  interconnected  by 
prismatic  joints  or,  more  often,  by  revolute  joints.  Some  of  these  bodies 
form  lower  limbs,  whose  extremities  (feet)  periodically  come  in  contact 
with  the  ground  to  realize  locomotion.  There  is  a  large  variety  of  mechan¬ 
ical  structures  in  this  class,  whose  design  is  often  inspired  by  the  study  of 
living  organisms  (biomimetic  robotics):  they  range  from  biped  humanoids 
to  hexapod  robots  aimed  at  replicating  the  biomechanical  efficiency  of 
insects. 

Only  wheeled  vehicles  are  considered  in  the  following,  as  they  represent 
the  vast  majority  of  mobile  robots  actually  used  in  applications.  The  basic 
mechanical  element  of  such  robots  is  indeed  the  wheel.  Three  types  of  con¬ 
ventional  wheels  exist,  which  are  shown  in  Fig.  1.12  together  with  the  icons 
that  will  be  used  to  represent  them: 

•  The  fixed  wheel  can  rotate  about  an  axis  that  goes  through  the  center 
of  the  wheel  and  is  orthogonal  to  the  wheel  plane.  The  wheel  is  rigidly 
attached  to  the  chassis,  whose  orientation  with  respect  to  the  wheel  is 
therefore  constant. 

•  The  steerable  wheel  has  two  axes  of  rotation.  The  first  is  the  same  as  a 
fixed  wheel,  while  the  second  is  vertical  and  goes  through  the  center  of  the 
wheel.  This  allows  the  wheel  to  change  its  orientation  with  respect  to  the 
chassis. 

•  The  caster  wheel  has  two  axes  of  rotation,  but  the  vertical  axis  does  not 
pass  through  the  center  of  the  wheel,  from  which  it  is  displaced  by  a  con¬ 
stant  offset.  Such  an  arrangement  causes  the  wheel  to  swivel  automatically, 
rapidly  aligning  with  the  direction  of  motion  of  the  chassis.  This  type  of 
wheel  is  therefore  introduced  to  provide  a  supporting  point  for  static  bal¬ 
ance  without  affecting  the  mobility  of  the  base;  for  instance,  caster  wheels 
are  commonly  used  in  shopping  carts  as  well  as  in  chairs  with  wheels. 
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Fig.  1.13.  A  differential-drive  mobile  robot 


Fig.  1.14.  A  synchro-drive  mobile  robot 


The  variety  of  kinematic  structures  that  can  be  obtained  by  combining 
the  three  conventional  wheels  is  wide.  In  the  following,  the  most  relevant 
arrangements  are  briefly  examined. 

In  a  differential- drive  vehicle  there  are  two  fixed  wheels  with  a  common 
axis  of  rotation,  and  one  or  more  caster  wheels,  typically  smaller,  whose  func¬ 
tion  is  to  keep  the  robot  statically  balanced  (Fig.  1.13).  The  two  fixed  wheels 
are  separately  controlled,  in  that  different  values  of  angular  velocity  may  be 
arbitrarily  imposed,  while  the  caster  wheel  is  passive.  Such  a  robot  can  rotate 
on  the  spot  (i.e.,  without  moving  the  midpoint  between  the  wheels),  provided 
that  the  angular  velocities  of  the  two  wheels  are  equal  and  opposite. 

A  vehicle  with  similar  mobility  is  obtained  using  a  synchro-drive  kinematic 
arrangement  (Fig.  1.14).  This  robot  has  three  aligned  steerable  wheels  which 
are  synchronously  driven  by  only  two  motors  through  a  mechanical  coupling, 
e.g.,  a  chain  or  a  transmission  belt.  The  first  motor  controls  the  rotation  of  the 
wheels  around  the  horizontal  axis,  thus  providing  the  driving  force  (traction) 
to  the  vehicle.  The  second  motor  controls  the  rotation  of  the  wheels  around 
the  vertical  axis,  hence  affecting  their  orientation.  Note  that  the  heading  of 
the  chassis  does  not  change  during  the  motion.  Often,  a  third  motor  is  used 
in  this  type  of  robot  to  rotate  independently  the  upper  part  of  the  chassis  (a 
turret)  with  respect  to  the  lower  part.  This  may  be  useful  to  orient  arbitrarily 
a  directional  sensor  (e.g.,  a  camera)  or  in  any  case  to  recover  an  orientation 
error. 

In  a  tricycle  vehicle  (Fig.  1.15)  there  are  two  fixed  wheels  mounted  on  a 
rear  axle  and  a  steerable  wheel  in  front.  The  fixed  wheels  are  driven  by  a  single 
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Fig.  1.15.  A  tricycle  mobile  robot 


Fig.  1.16.  A  car-like  mobile  robot 

motor  which  controls  their  traction,2  while  the  steerable  wheel  is  driven  by 
another  motor  which  changes  its  orientation,  acting  then  as  a  steering  device. 
Alternatively,  the  two  rear  wheels  may  be  passive  and  the  front  wheel  may 
provide  traction  as  well  as  steering. 

A  car-like  vehicle  has  two  fixed  wheels  mounted  on  a  rear  axle  and  two 
steerable  wheels  mounted  on  a  front  axle,  as  shown  in  Fig.  1.16.  As  in  the 
previous  case,  one  motor  provides  (front  or  rear)  traction  while  the  other 
changes  the  orientation  of  the  front  wheels  with  respect  to  the  vehicle.  It  is 
worth  pointing  out  that,  to  avoid  slippage,  the  two  front  wheels  must  have  a 
different  orientation  when  the  vehicle  moves  along  a  curve;  in  particular,  the 
internal  wheel  is  slightly  more  steered  with  respect  to  the  external  one.  This 
is  guaranteed  by  the  use  of  a  specific  device  called  Ackermann  steering. 

Finally,  consider  the  robot  in  Fig.  1.17,  which  has  three  caster  wheels 
usually  arranged  in  a  symmetric  pattern.  The  traction  velocities  of  the  three 
wheels  are  independently  driven.  Unlike  the  previous  cases,  this  vehicle  is  om¬ 
nidirectional'.  in  fact,  it  can  move  instantaneously  in  any  Cartesian  direction, 
as  well  as  re-orient  itself  on  the  spot. 

In  addition  to  the  above  conventional  wheels,  there  exist  other  special 
types  of  wheels,  among  which  is  notably  the  Mecanum  (or  Swedish)  wheel , 
shown  in  Fig.  1.18.  This  is  a  fixed  wheel  with  passive  rollers  placed  along  the 
external  rim;  the  axis  of  rotation  of  each  roller  is  typically  inclined  by  45°  with 
respect  to  the  plane  of  the  wheel.  A  vehicle  equipped  with  four  such  wheels 
mounted  in  pairs  on  two  parallel  axles  is  also  omnidirectional. 

2  The  distribution  of  the  traction  torque  on  the  two  wheels  must  take  into  account 
the  fact  that  in  general  they  move  with  different  speeds.  The  mechanism  which 
equally  distributes  traction  is  the  differential. 
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Fig.  1.17.  An  omnidirectional  mobile  robot  with  three  independently  driven  caster 
wheels 


Fig.  1.18.  A  Mecanum  (or  Swedish)  wheel 


In  the  design  of  a  wheeled  robot,  the  mechanical  balance  of  the  structure 
does  not  represent  a  problem  in  general.  In  particular,  a  three-wheel  robot  is 
statically  balanced  as  long  as  its  center  of  mass  falls  inside  the  support  triangle, 
which  is  defined  by  the  contact  points  between  the  wheels  and  ground.  Robots 
with  more  than  three  wheels  have  a  support  polygon ,  and  thus  it  is  typically 
easier  to  guarantee  the  above  balance  condition.  It  should  be  noted,  however, 
that  when  the  robot  moves  on  uneven  terrain  a  suspension  system  is  needed 
to  maintain  the  contact  between  each  wheel  and  the  ground. 

Unlike  the  case  of  manipulators,  the  workspace  of  a  mobile  robot  (defined 
as  the  portion  of  the  surrounding  environment  that  the  robot  can  access)  is  po¬ 
tentially  unlimited.  Nevertheless,  the  local  mobility  of  a  non-omnidirectional 
mobile  robot  is  always  reduced;  for  instance,  the  tricycle  robot  in  Fig.  1.15 
cannot  move  instantaneously  in  a  direction  parallel  to  the  rear  wheel  axle. 
Despite  this  fact,  the  tricycle  can  be  manoeuvered  so  as  to  obtain,  at  the  end 
of  the  motion,  a  net  displacement  in  that  direction.  In  other  words,  many 
mobile  robots  are  subject  to  constraints  on  the  admissible  instantaneous  mo¬ 
tions,  without  actually  preventing  the  possibility  of  attaining  any  position  and 
orientation  in  the  workspace.  This  also  implies  that  the  number  of  DOFs  of 
the  robot  (meant  as  the  number  of  admissible  instantaneous  motions)  is  lower 
than  the  number  of  its  configuration  variables. 

It  is  obviously  possible  to  merge  the  mechanical  structure  of  a  manipulator 
with  that  of  a  mobile  vehicle  by  mounting  the  former  on  the  latter.  Such 
a  robot  is  called  a  mobile  manipulator  and  combines  the  dexterity  of  the 
articulated  arm  with  the  unlimited  mobility  of  the  base.  An  example  of  such 
a  mechanical  structure  is  shown  in  Fig.  1.19.  However,  the  design  of  a  mobile 
manipulator  involves  additional  difficulties  related,  for  instance,  to  the  static 
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Fig.  1.19.  A  mobile  manipulator  obtained  by  mounting  an  anthropomorphic  arm 
on  a  differential-drive  vehicle 


and  dynamic  mechanical  balance  of  the  robot,  as  well  as  to  the  actuation  of 
the  two  systems. 


1.3  Industrial  Robotics 

Industrial  robotics  is  the  discipline  concerning  robot  design,  control  and  ap¬ 
plications  in  industry,  and  its  products  have  by  now  reached  the  level  of  a 
mature  technology.  The  connotation  of  a  robot  for  industrial  applications  is 
that  of  operating  in  a  structured  environment  whose  geometrical  or  physical 
characteristics  are  mostly  known  a  priori.  Hence,  limited  autonomy  is  required. 

The  early  industrial  robots  were  developed  in  the  1960s,  at  the  confluence 
of  two  technologies:  numerical  control  machines  for  precise  manufacturing, 
and  teleoperators  for  remote  radioactive  material  handling.  Compared  to  its 
precursors,  the  first  robot  manipulators  were  characterized  by: 

•  versatility,  in  view  of  the  employment  of  different  end-effectors  at  the  tip 
of  the  manipulator, 

•  adaptability  to  a  priori  unknown  situations,  in  view  of  the  use  of  sensors, 

•  positioning  accuracy,  in  view  of  the  adoption  of  feedback  control  tech¬ 
niques, 

•  execution  repeatability,  in  view  of  the  programmability  of  various  opera¬ 
tions. 

During  the  subsequent  decades,  industrial  robots  have  gained  a  wide  popu¬ 
larity  as  essential  components  for  the  realization  of  automated  manufacturing 


16 


1  Introduction 


80,000 


A 

P 


60,000  —33 - 55" 

40,000 
20,000 

1993  1994  1995  1996  1997  1998  1999  2000  2001  2002  2003  2004  2005  2006 

Fig.  1.20.  Yearly  installations  of  industrial  robots  worldwide 


systems.  The  main  factors  having  determined  the  spread  of  robotics  tech¬ 
nology  in  an  increasingly  wider  range  of  applications  in  the  manufacturing 
industry  are  reduction  of  manufacturing  costs,  increase  of  productivity,  im¬ 
provement  of  product  quality  standards  and,  last  but  not  least,  the  possibility 
of  eliminating  harmful  or  off-putting  tasks  for  the  human  operator  in  a  man¬ 
ufacturing  system. 

By  its  usual  meaning,  the  term  automation  denotes  a  technology  aimed  at 
replacing  human  beings  with  machines  in  a  manufacturing  process,  as  regards 
not  only  the  execution  of  physical  operations  but  also  the  intelligent  processing 
of  information  on  the  status  of  the  process.  Automation  is  then  the  synthesis 
of  industrial  technologies  typical  of  the  manufacturing  process  and  computer 
technology  allowing  information  management.  The  three  levels  of  automation 
one  may  refer  to  are  rigid  automation,  programmable  automation,  and  flexible 
automation. 

Rigid  automation  deals  with  a  factory  context  oriented  to  the  mass  manu¬ 
facture  of  products  of  the  same  type.  The  need  to  manufacture  large  numbers 
of  parts  with  high  productivity  and  quality  standards  demands  the  use  of 
fixed  operational  sequences  to  be  executed  on  the  workpiece  by  special  pur¬ 
pose  machines. 

Programmable  automation  deals  with  a  factory  context  oriented  to  the 
manufacture  of  low-to-medium  batches  of  products  of  different  types.  A  pro¬ 
grammable  automated  system  permits  changing  easy  the  sequence  of  opera¬ 
tions  to  be  executed  on  the  workpieces  in  order  to  vary  the  range  of  products. 
The  machines  employed  are  more  versatile  and  are  capable  of  manufacturing 
different  objects  belonging  to  the  same  group  technology.  The  majority  of  the 
products  available  on  the  market  today  are  manufactured  by  programmable 
automated  systems. 
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Fig.  1.21.  Yearly  supply  of  industrial  robots  by  main  industries 


Flexible  automation  represents  the  evolution  of  programmable  automation. 
Its  goal  is  to  allow  manufacturing  of  variable  batches  of  different  products  by 
minimizing  the  time  lost  for  reprogramming  the  sequence  of  operations  and 
the  machines  employed  to  pass  from  one  batch  to  the  next.  The  realization  of  a 
flexible  manufacturing  system  (FMS)  demands  strong  integration  of  computer 
technology  with  industrial  technology. 

The  industrial  robot  is  a  machine  with  significant  characteristics  of  versa¬ 
tility  and  flexibility.  According  to  the  widely  accepted  definition  of  the  Robot 
Institute  of  America,  a  robot  is  a  reprogrammable  multifunctional  manipulator 
designed  to  move  materials,  parts,  tools  or  specialized  devices  through  variable 
programmed  motions  for  the  performance  of  a  variety  of  tasks.  Such  a  defini¬ 
tion,  dating  back  to  1980,  reflects  the  current  status  of  robotics  technology. 

By  virtue  of  its  programmability,  the  industrial  robot  is  a  typical  com¬ 
ponent  of  programmable  automated  systems.  Nonetheless,  robots  can  be  en¬ 
trusted  with  tasks  in  both  rigid  and  flexible  automated  systems. 

According  to  the  above-mentioned  IFR  report,  up  to  2006  nearly  one  mil¬ 
lion  industrial  robots  are  in  use  worldwide,  half  of  which  are  in  Asia,  one  third 
in  Europe,  and  16%  in  North  America.  The  four  countries  with  the  largest 
number  of  robots  are  Japan,  Germany,  United  States  and  Italy.  The  figures 
for  robot  installations  in  the  last  15  years  are  summarized  in  the  graph  in 
Fig.  1.20;  by  the  end  of  2007,  an  increase  of  10%  in  sales  with  respect  to  the 
previous  year  is  foreseen,  with  milder  increase  rates  in  the  following  years, 
reaching  a  worldwide  figure  of  1,200,000  units  at  work  by  the  end  of  2010. 

In  the  same  report  it  is  shown  how  the  average  service  life  of  an  industrial 
robot  is  about  12  years,  which  may  increase  to  15  in  a  few  years  from  now. 
An  interesting  statistic  is  robot  density  based  on  the  total  number  of  persons 
employed:  this  ranges  from  349  robots  in  operation  per  10,000  workers  to 
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Fig.  1.22.  Examples  of  AGVs  for  material  handling  (courtesy  of  E&K  Automation 
GmbH) 


187  in  Korea,  186  in  Germany,  and  13  in  Italy.  The  United  States  has  just 
99  robots  per  10,000  workers.  The  average  cost  of  a  6-axis  industrial  robot, 
including  the  control  unit  and  development  software,  ranges  from  20,000  to 
60,000  euros,  depending  on  the  size  and  applications. 

The  automotive  industry  is  still  the  predominant  user  of  industrial  robots. 
The  graph  in  Fig.  1.21  referring  to  2005  and  2006,  however,  reveals  how  both 
the  chemical  industry  and  the  electrical/electronics  industry  are  gaining  in  im¬ 
portance,  and  new  industrial  applications,  such  as  metal  products,  constitute 
an  area  with  a  high  potential  investment. 

Industrial  robots  present  three  fundamental  capacities  that  make  them 
useful  for  a  manufacturing  process:  material  handling ,  manipulation ,  and  mea¬ 
surement. 

In  a  manufacturing  process,  each  object  has  to  be  transferred  from  one 
location  in  the  factory  to  another  in  order  to  be  stored,  manufactured,  assem¬ 
bled,  and  packed.  During  transfer,  the  physical  characteristics  of  the  object  do 
not  undergo  any  alteration.  The  robot’s  capability  to  pick  up  an  object,  move 
it  in  space  on  predefined  paths  and  release  it  makes  the  robot  itself  an  ideal 
candidate  for  material  handling  operations.  Typical  applications  include: 

•  palletizing  (placing  objects  on  a  pallet  in  an  ordered  way), 

•  warehouse  loading  and  unloading, 

•  mill  and  machine  tool  tending, 

•  part  sorting, 

•  packaging. 

In  these  applications,  besides  robots,  Automated  Guided  Vehicles  (AGV) 
are  utilized  which  ensure  handling  of  parts  and  tools  around  the  shop  floor 
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Fig.  1.23.  Yearly  supply  of  industrial  robots  in  Europe  for  manufacturing  operations 


from  one  manufacturing  cell  to  the  next  (Fig.  1.22).  As  compared  to  the  tra¬ 
ditional  fixed  guide  paths  for  vehicles  (inductive  guide  wire,  magnetic  tape, 
or  optical  visible  line),  modern  AG  Vs  utilize  high-tech  systems  with  onboard 
microprocessors  and  sensors  (laser,  odometry,  GPS)  which  allow  their  local¬ 
ization  within  the  plant  layout,  and  manage  their  work  flow  and  functions, 
allowing  their  complete  integration  in  the  FMS.  The  mobile  robots  employed 
in  advanced  applications  can  be  considered  as  the  natural  evolution  of  the 
AGV  systems,  as  far  as  enhanced  autonomy  is  concerned. 

Manufacturing  consists  of  transforming  objects  from  raw  material  into 
finished  products;  during  this  process,  the  part  either  changes  its  own  physical 
characteristics  as  a  result  of  machining,  or  loses  its  identity  as  a  result  of  an 
assembly  of  more  parts.  The  robot’s  capability  to  manipulate  both  objects  and 
tools  make  it  suitable  to  be  employed  in  manufacturing.  Typical  applications 
include: 

•  arc  and  spot  welding, 

•  painting  and  coating, 

•  gluing  and  sealing, 

•  laser  and  water  jet  cutting, 

•  milling  and  drilling, 

•  casting  and  die  spraying, 

•  deburring  and  grinding, 

•  screwing,  wiring  and  fastening, 

•  assembly  of  mechanical  and  electrical  groups, 

•  assembly  of  electronic  boards. 
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Fig.  1.24.  The  AdeptOne  XL  robot  (courtesy  of  Adept  Technology  Inc) 


Besides  material  handling  and  manipulation,  in  a  manufacturing  process 
it  is  necessary  to  perform  measurements  to  test  product  quality.  The  robot’s 
capability  to  explore  3D  space  together  with  the  availability  of  measurements 
on  the  manipulator’s  status  allow  a  robot  to  be  used  as  a  measuring  device. 
Typical  applications  include: 

•  object  inspection, 

•  contour  finding, 

•  detection  of  manufacturing  imperfections. 

The  graph  in  Fig.  1.23  reports  the  number  of  robots  employed  in  Europe 
in  2005  and  2006  for  various  operations,  which  reveals  how  material  handling 
requires  twice  as  many  robots  employed  for  welding,  whereas  a  limited  number 
of  robots  is  still  employed  for  assembly. 

In  the  following  some  industrial  robots  are  illustrated  in  terms  of  their 
features  and  application  fields. 

The  AdeptOne  XL  robot  in  Fig.  1.24  has  a  four-joint  SCARA  structure. 
Direct  drive  motors  are  employed.  The  maximum  reach  is  800  mm,  with  a 
repeatability  of  0.025  mm  horizontally  and  0.038  mm  vertically.  Maximum 
speeds  are  1200  mm/s  for  the  prismatic  joint,  while  they  range  from  to  650 
to  3300  deg/s  for  the  three  revolute  joints.  The  maximum  payload3  is  12  kg. 
Typical  industrial  applications  include  small-parts  material  handling,  assem¬ 
bly  and  packaging. 


3  Repeatability  and  payload  are  classical  parameters  found  in  industrial  robot  data 
sheets.  The  former  gives  a  measure  of  the  manipulator’s  ability  to  return  to  a 
previously  reached  position,  while  the  latter  indicates  the  average  load  to  be 
carried  at  the  robot’s  end-effector. 
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Fig.  1.25.  The  COMAU  Smart  NS  robot  (courtesy  of  COMAU  SpA  Robotica) 


Fig.  1.26.  The  ABB  IRB  4400  robot  (courtesy  of  ABB  Robotics) 


The  Comau  SMART  NS  robot  in  Fig.  1.25  has  a  six-joint  anthropomorphic 
structure  with  spherical  wrist.  In  its  four  versions,  the  outreach  ranges  from 
1650  and  1850  mm  horizontally,  with  a  repeatability  of  0.05  mm.  Maximum 
speeds  range  from  155  to  170  deg/s  for  the  inner  three  joints,  and  from  350 
to  550  deg/s  for  the  outer  three  joints.  The  maximum  payload  is  16  kg.  Both 
floor  and  ceiling  mounting  positions  are  allowed.  Typical  industrial  applica¬ 
tions  include  arc  welding,  light  handling,  assembly  and  technological  processes. 

The  ABB  IRB  4400  robot  in  Fig.  1.26  also  has  a  six-joint  anthropomor¬ 
phic  structure,  but  unlike  the  previous  open-chain  structure,  it  possesses  a 
closed  chain  of  parallelogram  type  between  the  shoulder  and  elbow  joints. 
The  outreach  ranges  from  1960  to  2550  mm  for  the  various  versions,  with  a 
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Fig.  1.27.  The  KUKA  KR  60  Jet  robot  (courtesy  of  KUKA  Roboter  GmbH) 


repeatability  from  0.07  to  0.1mm.  The  maximum  speed  at  the  end-effector 
is  2200  mm/s.  The  maximum  payload  is  60  kg.  Floor  or  shelf-mounting  is 
available.  Typical  industrial  applications  include  material  handling,  machine 
tending,  grinding,  gluing,  casting,  die  spraying  and  assembly. 

The  KUKA  KR  60  Jet  robot  in  Fig.  1.27  is  composed  of  a  five-axis  struc¬ 
ture,  mounted  on  a  sliding  track  with  a  gantry-type  installation;  the  upright 
installation  is  also  available.  The  linear  unit  has  a  stroke  from  a  minimum 
of  400  mm  to  a  maximum  of  20  m  (depending  on  customer’s  request),  and  a 
maximum  speed  of  3200  mm/s.  On  the  other  hand,  the  robot  has  a  payload 
of  60  kg,  an  outreach  of  820  mm  and  a  repeatability  of  0.15  mm.  Maximum 
speeds  are  120  deg/s  and  166  deg/s  for  the  first  two  joints,  while  they  range 
from  260  to  322  deg/s  for  the  outer  three  joints.  Typical  industrial  applications 
include  machine  tending,  arc  welding,  deburring,  coating,  sealing,  plasma  and 
waterjet  cutting. 

The  ABB  IRB340  FlexPicker  robot  in  Fig.  1.28  adopts  a  parallel  geometry 
with  four  axes;  in  view  of  its  reduced  weight  and  floor  mounting,  the  robot 
can  transport  150  objects  a  minute  (cycle  time  of  just  0.4  s),  reaching  record 
speeds  of  10  m/s  and  accelerations  of  100m/s2,  for  a  payload  of  1kg,  with 
a  repeatability  of  0.1mm.  In  its  ‘clean’  aluminum  version,  it  is  particularly 
suitable  for  packaging  in  the  food  and  pharmaceutical  industries. 

The  Fanuc  M-16iB  robot  in  Fig.  1.29  has  a  six-joint  anthropomorphic 
structure  with  a  spherical  wrist.  In  its  two  versions,  the  outreach  varies 
from  1667  to  1885  mm  horizontally,  with  a  repeatability  of  0.1mm.  Maximum 
speeds  range  from  165  to  175  deg/s  for  the  inner  three  joints,  and  from  340 
to  520  deg/s  for  the  outer  three  joints.  Payload  varies  from  10  to  20  kg.  The 
peculiarity  of  this  robot  consists  of  the  integrated  sensors  in  the  control  unit, 
including  a  servoing  system  based  on  3D  vision  and  a  six-axis  force  sensor. 
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Fig.  1.28.  The  ABB  IRB  340  FlexPicker  robot  (courtesy  of  ABB  Robotics) 


Fig.  1.29.  The  Fanuc  M-16iB  robot  (courtesy  of  Fanuc  Ltd) 


The  robot  is  utilized  for  handling  arbitrarily  located  objects,  deburring,  seal¬ 
ing  and  waterjet  cutting. 

The  Light  Weight  Robot  (LWR)  in  Fig.  1.30  with  a  seven-axis  structure 
was  introduced  in  2006  as  the  outcome  of  technology  transfer  from  DLR  (the 
German  Aerospace  Agency)  to  KUKA.  In  view  of  the  adoption  of  lightweight 
materials,  as  well  as  the  adoption  of  torque  sensors  at  the  joints,  the  robot 
can  manipulate  a  payload  of  7  to  14  kg,  in  the  face  of  a  weight  of  the  structure 
of  just  15  kg.  The  horizontal  outreach  is  868  mm,  with  joint  speeds  ranging 
from  110  to  210  deg/s.  On  the  other  hand,  the  presence  of  the  seventh  axis  of 
motion  confers  kinematic  redundancy  to  the  robot,  which  can  then  be  recon¬ 
figured  into  more  dexterous  postures  for  the  execution  of  given  tasks.  Such 
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Fig.  1.30.  The  KUKA  LWR  robot  (courtesy  of  KUKA  Roboter  GmbH) 


a  manipulator  represents  one  of  the  most  advanced  industrial  products  and, 
in  view  of  its  lightweight  feature,  it  offers  interesting  performance  for  interac¬ 
tion  with  the  environment,  ensuring  an  inherent  safety  in  case  of  contact  with 
human  beings. 

In  most  industrial  applications  requiring  object  manipulation,  typical  grip¬ 
pers  are  utilized  as  end-effectors.  Nevertheless,  whenever  enhanced  manipula- 
bility  and  dexterity  is  desired,  multifingered  robot  hands  are  available. 

The  BarrettHand  (Fig.  1.31),  endowed  with  a  fixed  finger  and  two  mobile 
fingers  around  the  base  of  the  palm,  allows  the  manipulation  of  objects  of 
different  dimension,  shape  and  orientation. 

The  SCHUNK  Antropomorphic  Hand  (SAH)  in  Fig.  1.32  is  the  outcome 
of  technology  transfer  from  DLR  and  Harbin  Institute  of  Technology  (China) 
to  SCHUNK.  Characterized  by  three  independent  aligned  fingers  and  an  op¬ 
posing  finger  which  is  analogous  to  the  human  thumb.  The  finger  joints  are 
endowed  with  magnetic  angular  sensors  and  torque  sensors.  This  hand  offers 
good  dexterity  and  approaches  the  characteristics  of  the  human  hand. 

LWR  technology  has  been  employed  for  the  realization  of  the  two  arms 
of  Justin,  a  humanoid  manipulator  made  by  DLR,  composed  of  a  three-joint 
torso  with  an  anthropomorphic  structure,  two  seven-axis  arms  and  a  sen- 
sorized  head.  The  robot  is  illustrated  in  Fig.  1.33  in  the  execution  of  a  biman¬ 
ual  manipulation  task;  the  hands  employed  are  previous  versions  of  the  SAH 
anthropomorphic  hand. 

The  applications  listed  describe  the  current  employment  of  robots  as  com¬ 
ponents  of  industrial  automation  systems.  They  all  refer  to  strongly  structured 
working  environments  and  thus  do  not  exhaust  all  the  possible  utilizations  of 
robots  for  industrial  applications.  Whenever  it  is  desired  to  tackle  problems 
requiring  the  adaptation  of  the  robot  to  a  changeable  working  environment, 
the  fall-out  of  advanced  robotics  products  are  of  concern.  In  this  regard,  the 
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Fig.  1.31.  The  BarrettHand  (courtesy  of  Barrett  Technology  Inc) 


Fig.  1.32.  The  SCHUNK  Anthropomorphic  Hand  (courtesy  of  SCHUNK  Intec  Ltd) 


lightweight  robot,  the  hands  and  the  humanoid  manipulator  presented  above 
are  to  be  considered  at  the  transition  from  traditional  industrial  robotics  sys¬ 
tems  toward  those  innovative  systems  of  advanced  robotics. 


1.4  Advanced  Robotics 

The  expression  advanced  robotics  usually  refers  to  the  science  studying  robots 
with  marked  characteristics  of  autonomy ,  operating  in  scarcely  structured 
or  unstructured  environments ,  whose  geometrical  or  physical  characteristics 
would  not  be  known  a  priori. 

Nowadays,  advanced  robotics  is  still  in  its  youth.  It  has  indeed  featured 
the  realization  of  prototypes  only,  because  the  associated  technology  is  not 
yet  mature.  There  are  many  motivations  which  strongly  encourage  advances 
in  knowledge  within  this  field.  They  range  from  the  need  for  automata  when¬ 
ever  human  operators  are  not  available  or  are  not  safe  ( field  robots),  to  the 
opportunity  of  developing  products  for  potentially  wide  markets  which  are 
aimed  at  improving  quality  of  life  ( service  robots). 

The  graph  in  Fig.  1.34  reports  the  number  of  robots  in  stock  for  non¬ 
industrial  applications  at  the  end  of  2006  and  the  forecast  to  2010.  Such 
applications  are  characterized  by  the  complexity  level,  the  uncertainty  and 
variability  of  the  environment  with  which  the  robot  interacts,  as  shown  in  the 
following  examples. 
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Fig.  1.33.  The  Justin  humanoid  robot  manipulator  (courtesy  of  DLR) 


1.4.1  Field  Robots 

The  context  is  that  of  deploying  robots  in  areas  where  human  beings  could 
not  survive  or  be  exposed  to  unsustainable  risks.  Such  robots  should  carry 
out  exploration  tasks  and  report  useful  data  on  the  environment  to  a  remote 
operator,  using  suitable  onboard  sensors.  Typical  scenarios  are  the  explo¬ 
ration  of  a  volcano,  the  intervention  in  areas  contaminated  by  poisonous  gas 
or  radiation,  or  the  exploration  of  the  deep  ocean  or  space.  As  is  well  known, 
NASA  succeeded  in  delivering  some  mobile  robots  (rovers)  to  Mars  (Fig.  1.35) 
which  navigated  on  the  Martian  soil,  across  rocks,  hills  and  crevasses.  Such 
rovers  were  partially  teleoperated  from  earth  and  have  successfully  explored 
the  environment  with  sufficient  autonomy.  Some  mini-robots  were  deployed 
on  September  11,  2001  at  Ground  Zero  after  the  collapse  of  the  Twin  Towers 
in  New  York,  to  penetrate  the  debris  in  the  search  for  survivors. 

A  similar  scenario  is  that  of  disasters  caused  by  fires  in  tunnels  or  earth¬ 
quakes;  in  such  occurrences,  there  is  a  danger  of  further  explosions,  escape  of 
harmful  gases  or  collapse,  and  thus  human  rescue  teams  may  cooperate  with 
robot  rescue  teams.  Also  in  the  military  field,  unmanned  autonomous  aircrafts 
and  missiles  are  utilized,  as  well  as  teleoperated  robots  with  onboard  cameras 
to  explore  buildings.  The  ‘Grand  Challenge’  of  October  2005  (Fig.  1.36)  was 
financially  supported  by  the  US  Department  of  Defense  (DARPA)  with  the 
goal  of  developing  autonomous  vehicles  to  carry  weapons  and  sensors,  thus 
reducing  soldier  employment. 
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Fig.  1.34.  Robots  on  stock  for  non-industrial  applications 


Fig.  1.35.  The  Sojourner  rover  was  deployed  by  the  Pathfinder  lander  and  explored 
250  m2  of  Martian  soil  in  1997  (courtesy  of  NASA) 


1.4.2  Service  Robots 

Autonomous  vehicles  are  also  employed  for  civil  applications,  i.e.,  for  mass 
transit  systems  (Fig.  1.37),  thus  contributing  to  the  reduction  of  pollution 
levels.  Such  vehicles  are  part  of  the  so-called  Intelligent  Transportation  Sys¬ 
tems  (ITS)  devoted  to  traffic  management  in  urban  areas.  Another  feasible 
application  where  the  adoption  of  mobile  robots  offers  potential  advantages 
is  museum  guided  tours  (Fig.  1.38). 

Many  countries  are  investing  in  establishing  the  new  market  of  service 
robots  which  will  co-habitat  with  human  beings  in  everyday  life.  According 
to  the  above-mentioned  IFR  report,  up  to  2005  1.9  million  service  robots  for 
domestic  applications  (Fig.  1.39)  and  1  million  toy  robots  have  been  sold. 

Technology  is  ready  to  transform  into  commercial  products  the  prototypes 
of  robotic  aids  to  enhance  elderly  and  impaired  people’s  autonomy  in  everyday 
life;  autonomous  wheelchairs,  mobility  aid  lifters,  feeding  aids  and  rehabilita¬ 
tion  robots  allowing  tetraplegics  to  perform  manual  labor  tasks  are  examples 
of  such  service  devices.  In  perspective,  other  than  an  all-purpose  robot  waiter, 
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Fig.  1.36.  The  unmanned  car  Stanley  autonomously  completed  a  path  of  132  miles 
in  the  record  time  of  6  h  and  53  min  (courtesy  of  DARPA) 


Fig.  1.37.  The  Cycab  is  an  electrically-driven  vehicle  for  autonomous  transportation 
in  urban  environments  (courtesy  of  INRIA) 


assistance,  and  healthcare  systems  integrating  robotic  and  telematic  modules 
will  be  developed  for  home  service  management  (domotics). 

Several  robotic  systems  are  employed  for  medical  applications.  Surgery 
assistance  systems  exploit  a  robot’s  high  accuracy  to  position  a  tool,  i.e. ,  for 
hip  prosthesis  implant.  Yet,  in  minimally-invasive  surgery,  i.e.,  cardiac  surgery, 
the  surgeon  operates  while  seated  comfortably  at  a  console  viewing  a  3D  image 
of  the  surgical  field,  and  operating  the  surgical  instruments  remotely  by  means 
of  a  haptic  interface  (Fig.  1.40). 

Further,  in  diagnostic  and  endoscopic  surgery  systems,  small  teleoperated 
robots  travels  through  the  cavities  of  human  body,  i.e.,  in  the  gastrointestinal 
system,  bringing  live  images  or  intervening  in  situ  for  biopsy,  dispensing  drugs 
or  removing  neoplasms. 
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Fig.  1.38.  Rhino,  employing  the  synchro-drive  mobile  base  B21  by  Real  World 
Interface,  was  one  of  the  first  robots  for  museum  guided  tours  (courtesy  of  Deutsches 
Museum  Bonn) 


Fig.  1.39.  The  vacuum  robot  Roomba,  employing  a  differential-drive  kinematics, 
autonomously  sweeps  and  cleans  floors  (courtesy  of  I-Robot  Corp) 


Finally,  in  motor  rehabilitation  systems,  a  hemiplegic  patient  wears  an 
exoskeleton,  which  actively  interacts,  sustains  and  corrects  the  movements 
according  to  the  physiotherapist’s  programmed  plan. 

Another  wide  market  segment  comes  from  entertainment,  where  robots 
are  used  as  toy  companions  for  children,  and  life  companions  for  the  elderly, 
such  as  humanoid  robots  (Fig.  1.41)  and  the  pet  robots  (Fig.  1.42)  being 
developed  in  Japan.  It  is  reasonable  to  predict  that  service  robots  will  be 
naturally  integrated  into  our  society.  Tomorrow,  robots  will  be  as  pervasive 
and  personal  as  today’s  personal  computers,  or  just  as  TV  sets  in  the  homes 
of  20  years  ago.  Robotics  will  then  become  ubiquitous,  a  challenge  under 
discussion  within  the  scientific  community. 


1.5  Robot  Modelling,  Planning  and  Control 

In  all  robot  applications,  completion  of  a  generic  task  requires  the  execution 
of  a  specific  motion  prescribed  to  the  robot.  The  correct  execution  of  such 
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Fig.  1.40.  The  da  Vinci  robotic  system  for  laparoscopic  surgery  (courtesy  of  Intu¬ 
itive  Surgical  Inc) 


motion  is  entrusted  to  the  control  system  which  should  provide  the  robot’s 
actuators  with  the  commands  consistent  with  the  desired  motion.  Motion 
control  demands  an  accurate  analysis  of  the  characteristics  of  the  mechanical 
structure,  actuators,  and  sensors.  The  goal  of  such  analysis  is  the  derivation 
of  the  mathematical  models  describing  the  input/output  relationship  charac¬ 
terizing  the  robot  components.  Modelling  a  robot  manipulator  is  therefore  a 
necessary  premise  to  finding  motion  control  strategies. 

Significant  topics  in  the  study  of  modelling,  planning  and  control  of  robots 
which  constitute  the  subject  of  subsequent  chapters  are  illustrated  below. 

1.5.1  Modelling 

Kinematic  analysis  of  the  mechanical  structure  of  a  robot  concerns  the  de¬ 
scription  of  the  motion  with  respect  to  a  fixed  reference  Cartesian  frame 
by  ignoring  the  forces  and  moments  that  cause  motion  of  the  structure.  It 
is  meaningful  to  distinguish  between  kinematics  and  differential  kinematics. 
With  reference  to  a  robot  manipulator,  kinematics  describes  the  analytical 
relationship  between  the  joint  positions  and  the  end-effector  position  and  ori¬ 
entation.  Differential  kinematics  describes  the  analytical  relationship  between 
the  joint  motion  and  the  end-effector  motion  in  terms  of  velocities,  through 
the  manipulator  Jacobiann. 

The  formulation  of  the  kinematics  relationship  allows  the  study  of  two 
key  problems  of  robotics,  namely,  the  direct  kinematics  problem  and  the  in¬ 
verse  kinematics  problem.  The  former  concerns  the  determination  of  a  sys¬ 
tematic,  general  method  to  describe  the  end-effector  motion  as  a  function  of 
the  joint  motion  by  means  of  linear  algebra  tools.  The  latter  concerns  the 
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Fig.  1.41.  The  Asimo  humanoid  robot,  launched  in  1996,  has  been  endowed  with 
even  more  natural  locomotion  and  human-robot  interaction  skills  (courtesy  of  Honda 
Motor  Company  Ltd) 


Fig.  1.42.  The  AIBO  dog  had  been  the  most  widely  diffused  entertainment  robot 
in  the  recent  years  (courtesy  of  Sony  Corp) 


inverse  problem;  its  solution  is  of  fundamental  importance  to  transform  the 
desired  motion,  naturally  prescribed  to  the  end-effector  in  the  workspace,  into 
the  corresponding  joint  motion. 

The  availability  of  a  manipulator’s  kinematic  model  is  also  useful  to  de¬ 
termine  the  relationship  between  the  forces  and  torques  applied  to  the  joints 
and  the  forces  and  moments  applied  to  the  end-effector  in  static  equilibrium 
configurations. 

Chapter  2  is  dedicated  to  the  study  of  kinematics.  Chapter  3  is  dedicated  to 
the  study  of  differential  kinematics  and  statics,  whereas  Appendix  A  provides 
a  useful  brush-up  on  linear  algebra. 

Kinematics  of  a  manipulator  represents  the  basis  of  a  systematic,  general 
derivation  of  its  dynamics,  i.e.,  the  equations  of  motion  of  the  manipulator 
as  a  function  of  the  forces  and  moments  acting  on  it.  The  availability  of  the 
dynamic  model  is  very  useful  for  mechanical  design  of  the  structure,  choice 
of  actuators,  determination  of  control  strategies,  and  computer  simulation  of 
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manipulator  motion.  Chapter  7  is  dedicated  to  the  study  of  dynamics,  whereas 
Appendix  B  recalls  some  fundamentals  on  rigid  body  mechanics. 

Modelling  of  mobile  robots  requires  a  preliminary  analysis  of  the  kinematic 
constraints  imposed  by  the  presence  of  wheels.  Depending  on  the  mechanical 
structure,  such  constraints  can  be  integrable  or  not;  this  has  direct  conse¬ 
quence  on  a  robot’s  mobility.  The  kinematic  model  of  a  mobile  robot  is  es¬ 
sentially  the  description  of  the  admissible  instantaneous  motions  in  respect 
of  the  constraints.  On  the  other  hand,  the  dynamic  model  accounts  for  the 
reaction  forces  and  describes  the  relationship  between  the  above  motions  and 
the  generalized  forces  acting  on  the  robot.  These  models  can  be  expressed 
in  a  canonical  form  which  is  convenient  for  design  of  planning  and  control 
techniques.  Kinematic  and  dynamic  analysis  of  mobile  robots  is  developed 
in  Chap.  11,  while  Appendix  D  contains  some  useful  concepts  of  differential 
geometry. 

1.5.2  Planning 

With  reference  to  the  tasks  assigned  to  a  manipulator,  the  issue  is  whether 
to  specify  the  motion  at  the  joints  or  directly  at  the  end-effector.  In  material 
handling  tasks,  it  is  sufficient  to  assign  only  the  pick-up  and  release  locations 
of  an  object  (point-to-point  motion),  whereas,  in  machining  tasks,  the  end- 
effector  has  to  follow  a  desired  trajectory  (path  motion).  The  goal  of  trajectory 
planning  is  to  generate  the  timing  laws  for  the  relevant  variables  (joint  or  end- 
effector)  starting  from  a  concise  description  of  the  desired  motion.  Chapter  4 
is  dedicated  to  trajectory  planning  for  robot  manipulators. 

The  motion  planning  problem  for  a  mobile  robot  concerns  the  generation 
of  trajectories  to  take  the  vehicle  from  a  given  initial  configuration  to  a  desired 
final  configuration.  Such  a  problem  is  more  complex  than  that  of  robot  ma¬ 
nipulators,  since  trajectories  have  to  be  generated  in  respect  of  the  kinematic 
constraints  imposed  by  the  wheels.  Some  solution  techniques  are  presented  in 
Chap.  11,  which  exploit  the  specific  differential  structure  of  the  mobile  robots’ 
kinematic  models. 

Whenever  obstacles  are  present  in  a  mobile  robot’s  workspace,  the  planned 
motions  must  be  safe,  so  as  to  avoid  collisions.  Such  a  problem,  known  as 
motion  planning ,  can  be  formulated  in  an  effective  fashion  for  both  robot  ma¬ 
nipulators  and  mobile  robots  utilizing  the  configuration  space  concept.  The 
solution  techniques  are  essentially  of  algorithmic  nature  and  include  exact, 
probabilistic  and  heuristic  methods.  Chapter  12  is  dedicated  to  motion  plan¬ 
ning  problem,  while  Appendix  E  provides  some  basic  concepts  on  graph  search 
algorithms. 

1.5.3  Control 

Realization  of  the  motion  specified  by  the  control  law  requires  the  employment 
of  actuators  and  sensors.  The  functional  characteristics  of  the  most  commonly 
used  actuators  and  sensors  for  robots  are  described  in  Chap.  5. 
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Chapter  6  is  concerned  with  the  hardware/software  architecture  of  a 
robot’s  control  system  which  is  in  charge  of  implementation  of  control  laws  as 
well  as  of  interface  with  the  operator. 

The  trajectories  generated  constitute  the  reference  inputs  to  the  motion 
control  system  of  the  mechanical  structure.  The  problem  of  robot  manipulator 
control  is  to  find  the  time  behaviour  of  the  forces  and  torques  to  be  delivered 
by  the  joint  actuators  so  as  to  ensure  the  execution  of  the  reference  trajec¬ 
tories.  This  problem  is  quite  complex,  since  a  manipulator  is  an  articulated 
system  and,  as  such,  the  motion  of  one  link  influences  the  motion  of  the  oth¬ 
ers.  Manipulator  equations  of  motion  indeed  reveal  the  presence  of  coupling 
dynamic  effects  among  the  joints,  except  in  the  case  of  a  Cartesian  structure 
with  mutually  orthogonal  axes.  The  synthesis  of  the  joint  forces  and  torques 
cannot  be  made  on  the  basis  of  the  sole  knowledge  of  the  dynamic  model, 
since  this  does  not  completely  describe  the  real  structure.  Therefore,  manip¬ 
ulator  control  is  entrusted  to  the  closure  of  feedback  loops;  by  computing  the 
deviation  between  the  reference  inputs  and  the  data  provided  by  the  propri¬ 
oceptive  sensors,  a  feedback  control  system  is  capable  of  satisfying  accuracy 
requirements  on  the  execution  of  the  prescribed  trajectories. 

Chapter  8  is  dedicated  to  the  presentation  of  motion  control  techniques, 
whereas  Appendix  C  illustrates  the  basic  principles  of  feedback  control. 

Control  of  a  mobile  robot  substantially  differs  from  the  analogous  problem 
for  robot  manipulators.  This  is  due,  in  turn,  to  the  availability  of  fewer  control 
inputs  than  the  robot  has  configuration  variables.  An  important  consequence 
is  that  the  structure  of  a  controller  allowing  a  robot  to  follow  a  trajectory 
(tracking  problem)  is  unavoidably  different  from  that  of  a  controller  aimed  at 
taking  the  robot  to  a  given  configuration  (regulation  problem).  Further,  since 
a  mobile  robot’s  proprioceptive  sensors  do  not  yield  any  data  on  the  vehicle’s 
configuration,  it  is  necessary  to  develop  localization  methods  for  the  robot 
in  the  environment.  The  control  design  problem  for  wheeled  mobile  robots  is 
treated  in  Chap.  11. 

If  a  manipulation  task  requires  interaction  between  the  robot  and  the  en¬ 
vironment,  the  control  problem  should  account  for  the  data  provided  by  the 
exteroceptive  sensors;  the  forces  exchanged  at  the  contact  with  the  environ¬ 
ment,  and  the  objects’  position  as  detected  by  suitable  cameras.  Chapter  9  is 
dedicated  to  force  control  techniques  for  robot  manipulators,  while  Chap.  10 
presents  visual  control  techniques. 
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Kinematics 


A  manipulator  can  be  schematically  represented  from  a  mechanical  viewpoint 
as  a  kinematic  chain  of  rigid  bodies  (links)  connected  by  means  of  revolute 
or  prismatic  joints.  One  end  of  the  chain  is  constrained  to  a  base,  while  an 
end-effector  is  mounted  to  the  other  end.  The  resulting  motion  of  the  struc¬ 
ture  is  obtained  by  composition  of  the  elementary  motions  of  each  link  with 
respect  to  the  previous  one.  Therefore,  in  order  to  manipulate  an  object  in 
space,  it  is  necessary  to  describe  the  end-effector  position  and  orientation. 
This  chapter  is  dedicated  to  the  derivation  of  the  direct  kinematics  equation 
through  a  systematic,  general  approach  based  on  linear  algebra.  This  allows 
the  end-effector  position  and  orientation  (pose)  to  be  expressed  as  a  function 
of  the  joint  variables  of  the  mechanical  structure  with  respect  to  a  reference 
frame.  Both  open-chain  and  closed-chain  kinematic  structures  are  considered. 
With  reference  to  a  minimal  representation  of  orientation,  the  concept  of 
operational  space  is  introduced  and  its  relationship  with  the  joint  space  is  es¬ 
tablished.  Furthermore,  a  calibration  technique  of  the  manipulator  kinematic 
parameters  is  presented.  The  chapter  ends  with  the  derivation  of  solutions  to 
the  inverse  kinematics  problem,  which  consists  of  the  determination  of  the 
joint  variables  corresponding  to  a  given  end-effector  pose. 


2.1  Pose  of  a  Rigid  Body 

A  rigid  body  is  completely  described  in  space  by  its  position  and  orientation 
(in  brief  pose)  with  respect  to  a  reference  frame.  As  shown  in  Fig.  2.1,  let 
O-xyz  be  the  orthonormal  reference  frame  and  x,  y,  z  be  the  unit  vectors  of 
the  frame  axes. 

The  position  of  a  point  O'  on  the  rigid  body  with  respect  to  the  coordinate 
frame  O-xyz  is  expressed  by  the  relation 


o'  =  o'xx  +  o'yy  +  o'zz , 
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Fig.  2.1.  Position  and  orientation  of  a  rigid  body 


where  o'x,  o'y,  o'z  denote  the  components  of  the  vector  o'  S  IR3  along  the  frame 
axes;  the  position  of  O'  can  be  compactly  written  as  the  (3  x  1)  vector 

(2.1) 

Vector  o'  is  a  bound  vector  since  its  line  of  application  and  point  of  application 
are  both  prescribed,  in  addition  to  its  direction  and  norm. 

In  order  to  describe  the  rigid  body  orientation,  it  is  convenient  to  consider 
an  orthonormal  frame  attached  to  the  body  and  express  its  unit  vectors  with 
respect  to  the  reference  frame.  Let  then  O'-x'y1  z'  be  such  a  frame  with  origin 
in  O'  and  x',  y\  z'  be  the  unit  vectors  of  the  frame  axes.  These  vectors  are 
expressed  with  respect  to  the  reference  frame  O-xyz  by  the  equations: 

x'  =  x'xx  +  x'yy  +  x!zz 

y' =  y'xx  +  y'vy  +  y'zz  (2-2) 

z'  =  z'xx  +  z'yy  +  z'zz. 

The  components  of  each  unit  vector  are  the  direction  cosines  of  the  axes  of 
frame  O'-x'y' z'  with  respect  to  the  reference  frame  O-xyz. 


2.2  Rotation  Matrix 


By  adopting  a  compact  notation,  the  three  unit  vectors  in  (2.2)  describing  the 
body  orientation  with  respect  to  the  reference  frame  can  be  combined  in  the 
(3  x  3)  matrix 


/ T 

X  X 

fT 

y  x 

fT 

Z  1  X 

fT 

&  y 

fT 

y  y 

fT 

z  y 

fT 

X  1  Z 

fT 

y  z 

z'Tz 

R  = 


x'  y'  z‘ 


(2.3) 


2.2  Rotation  Matrix 


41 


which  is  termed  rotation  matrix. 

It  is  worth  noting  that  the  column  vectors  of  matrix  R  are  mutually  or¬ 
thogonal  since  they  represent  the  unit  vectors  of  an  orthonormal  frame,  i.e. , 

x'Ty'  =  0  y'Tz'  =  0  z'Tx'  =  0. 

Also,  they  have  unit  norm 

x'Tx'  =  1  y'Ty'  =  l  z'Tz'  =  l. 

As  a  consequence,  R  is  an  orthogonal  matrix  meaning  that 

RtR  =  I3  (2.4) 

where  I3  denotes  the  (3  x  3)  identity  matrix. 

If  both  sides  of  (2.4)  are  postmultiplied  by  the  inverse  matrix  R  1 ,  the 
useful  result  is  obtained: 

Rt  =  R1,  (2.5) 

that  is,  the  transpose  of  the  rotation  matrix  is  equal  to  its  inverse.  Further, 
observe  that  det(F£)  =  1  if  the  frame  is  right-handed,  while  det(H)  =  —  1  if 
the  frame  is  left-handed. 

The  above-defined  rotation  matrix  belongs  to  the  special  orthonormal 
group  SO(m)  of  the  real  (m  x  m)  matrices  with  othonormal  columns  and 
determinant  equal  to  1;  in  the  case  of  spatial  rotations  it  is  ?n  =  3,  whereas 
in  the  case  of  planar  rotations  it  is  m  =  2. 


2.2.1  Elementary  Rotations 

Consider  the  frames  that  can  be  obtained  via  elementary  rotations  of  the 
reference  frame  about  one  of  the  coordinate  axes.  These  rotations  are  positive 
if  they  are  made  counter-clockwise  about  the  relative  axis. 

Suppose  that  the  reference  frame  O-xyz  is  rotated  by  an  angle  a  about 
axis  z  (Fig.  2.2),  and  let  O-x'y'z'  be  the  rotated  frame.  The  unit  vectors  of 
the  new  frame  can  be  described  in  terms  of  their  components  with  respect 
to  the  reference  frame.  Consider  the  frames  that  can  be  obtained  via  elemen¬ 
tary  rotations  of  the  reference  frame  about  one  of  the  coordinate  axes.  These 
rotations  are  positive  if  they  are  made  counter-clockwise  about  the  relative 
axis. 

Suppose  that  the  reference  frame  O-xyz  is  rotated  by  an  angle  a  about 
axis  z  (Fig.  2.2),  and  let  O-x'y'z'  be  the  rotated  frame.  The  unit  vectors  of 
the  new  frame  can  be  described  in  terms  of  their  components  with  respect  to 
the  reference  frame,  i.e., 


cos  a 

—sin  a 

'O' 

sin  a 

y'  = 

cos  a 

*'  = 

0 

0 

0 

1 
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Fig.  2.2.  Rotation  of  frame  O-xyz  by  an  angle  a  about  axis  z 


Hence,  the  rotation  matrix  of  frame  O-x'y'  z'  with  respect  to  frame  O-xyz  is 

cos  a  —sin  a  0 

Rz  (a)  =  sin  a  cos  a  0  .  (2.6) 

0  0  1 

In  a  similar  manner,  it  can  be  shown  that  the  rotations  by  an  angle  (3 
about  axis  y  and  by  an  angle  7  about  axis  x  are  respectively  given  by 

cos  (3  0  sin  [3 

Ry(f3)  =010  (2.7) 

— sin/3  0  cos  (3 

10  0  ' 

0  cosy  —sin  7  .  (2.8) 

0  sin  7  cos  7 

These  matrices  will  be  useful  to  describe  rotations  about  an  arbitrary  axis  in 

space. 

It  is  easy  to  verify  that  for  the  elementary  rotation  matrices  in  (2.6)-(2.8) 
the  following  property  holds: 

Rk(~fi)  =  Rk(d)  k  =  x,y,z.  (2.9) 

In  view  of  (2.6)-(2.8),  the  rotation  matrix  can  be  attributed  a  geometrical 
meaning;  namely,  the  matrix  R  describes  the  rotation  about  an  axis  in  space 
needed  to  align  the  axes  of  the  reference  frame  with  the  corresponding  axes 
of  the  body  frame. 

2.2.2  Representation  of  a  Vector 

In  order  to  understand  a  further  geometrical  meaning  of  a  rotation  matrix, 
consider  the  case  when  the  origin  of  the  body  frame  coincides  with  the  origin 


Rx{l)  = 
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Fig.  2.3.  Representation  of  a  point  P  in  two  different  coordinate  frames 


of  the  reference  frame  (Fig.  2.3);  it  follows  that  o'  =  0,  where  0  denotes  the 
(3  x  1)  null  vector.  A  point  P  in  space  can  be  represented  either  as 


P  = 


with  respect  to  frame  O-xyz ,  or  as 


P  = 


Px 

Py 

Pz 


Px 

Py 
L  p'z 


with  respect  to  frame  O-x'y'z' . 

Since  p  and  p ’  are  representations  of  the  same  point  P,  it  is 


P  =  PW  +  PyV'  +  Pzz'  = 


x'  y'  z' 


P 


and,  accounting  for  (2.3),  it  is 


p=Rp'.  (2.10) 

The  rotation  matrix  R  represents  the  transformation  matrix  of  the  vector 
coordinates  in  frame  O-x'y'z'  into  the  coordinates  of  the  same  vector  in  frame 
O-xyz.  In  view  of  the  orthogonality  property  (2.4),  the  inverse  transformation 
is  simply  given  by 


p’  =  RTp. 


(2.11) 
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Fig.  2.4.  Representation  of  a  point  P  in  rotated  frames 


Example  2.1 

Consider  two  frames  with  common  origin  mutually  rotated  by  an  angle  a  about 
the  axis  2.  Let  p  and  p'  be  the  vectors  of  the  coordinates  of  a  point  P,  expressed 
in  the  frames  O-xyz  and  O-x'y'z1,  respectively  (Fig.  2.4).  On  the  basis  of  simple 
geometry,  the  relationship  between  the  coordinates  of  P  in  the  two  frames  is 

Px  =  Px  cos  a  —  p'y  sin  a 
Py  =  Px  sin  a  +  p'y  cos  a 
Pz  =  Pz- 

Therefore,  the  matrix  (2.6)  represents  not  only  the  orientation  of  a  frame  with 
respect  to  another  frame,  but  it  also  describes  the  transformation  of  a  vector  from 
a  frame  to  another  frame  with  the  same  origin. 


2.2.3  Rotation  of  a  Vector 

A  rotation  matrix  can  be  also  interpreted  as  the  matrix  operator  allowing 
rotation  of  a  vector  by  a  given  angle  about  an  arbitrary  axis  in  space.  In  fact, 
let  p'  be  a  vector  in  the  reference  frame  O-xyz;  in  view  of  orthogonality  of  the 
matrix  R ,  the  product  Rp'  yields  a  vector  p  with  the  same  norm  as  that  of  p' 
but  rotated  with  respect  to  p!  according  to  the  matrix  R.  The  norm  equality 
can  be  proved  by  observing  that  pTp  =  plTRT Rp1  and  applying  (2.4).  This 
interpretation  of  the  rotation  matrix  will  be  revisited  later. 


2.3  Composition  of  Rotation  Matrices 
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Fig.  2.5.  Rotation  of  a  vector 


Example  2.2 

Consider  the  vector  p  which  is  obtained  by  rotating  a  vector  p'  in  the  plane  xy  by 
an  angle  a  about  axis  z  of  the  reference  frame  (Fig.  2.5).  Let  {p'x,p'y,p'z)  be  the 
coordinates  of  the  vector  p' .  The  vector  p  has  components 

px  =  Px  cos  a  —  p.y  sin  a 
Py  =  Px  sin  a+  p'y  cos  a 
Pz  =  pz- 

It  is  easy  to  recognize  that  p  can  be  expressed  as 

p  =  Rz(a)p , 

where  Rz(a)  is  the  same  rotation  matrix  as  in  (2.6). 


In  sum,  a  rotation  matrix  attains  three  equivalent  geometrical  meanings : 

•  It  describes  the  mutual  orientation  between  two  coordinate  frames;  its 
column  vectors  are  the  direction  cosines  of  the  axes  of  the  rotated  frame 
with  respect  to  the  original  frame. 

•  It  represents  the  coordinate  transformation  between  the  coordinates  of  a 
point  expressed  in  two  different  frames  (with  common  origin). 

•  It  is  the  operator  that  allows  the  rotation  of  a  vector  in  the  same  coordinate 
frame. 


2.3  Composition  of  Rotation  Matrices 

In  order  to  derive  composition  rules  of  rotation  matrices,  it  is  useful  to  consider 
the  expression  of  a  vector  in  two  different  reference  frames.  Let  then  O-xoyoZo, 
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O-XiyiZi,  O-X2IJ2Z2  be  three  frames  with  common  origin  O.  The  vector  p 
describing  the  position  of  a  generic  point  in  space  can  be  expressed  in  each 
of  the  above  frames;  let  p°,  p1,  p2  denote  the  expressions  of  p  in  the  three 
frames.1 

At  first,  consider  the  relationship  between  the  expression  p2  of  the  vector 
p  in  Frame  2  and  the  expression  pl  of  the  same  vector  in  Frame  1.  If  Rj 
denotes  the  rotation  matrix  of  Frame  i  with  respect  to  Frame  j,  it  is 

P 1  =  R\p 2-  (2.12) 

Similarly,  it  turns  out  that 

p°  =  R1P1 
p°  =  R2P2 

On  the  other  hand,  substituting  (2.12)  in  (2.13)  and  using  (2.14)  gives 

R°2  =  R^Rl  (2.15) 

The  relationship  in  (2.15)  can  be  interpreted  as  the  composition  of  successive 
rotations.  Consider  a  frame  initially  aligned  with  the  frame  0-xo2/o~o-  The 
rotation  expressed  by  matrix  R2  can  be  regarded  as  obtained  in  two  steps: 

•  First  rotate  the  given  frame  according  to  R®,  so  as  to  align  it  with  frame 

O-XiyiZi. 

•  Then  rotate  the  frame,  now  aligned  with  frame  O—XiyiZi,  according  to 
R\ ,  so  as  to  align  it  with  frame  0-x2y2Z2- 

Notice  that  the  overall  rotation  can  be  expressed  as  a  sequence  of  partial 
rotations;  each  rotation  is  defined  with  respect  to  the  preceding  one.  The 
frame  with  respect  to  which  the  rotation  occurs  is  termed  current  frame. 
Composition  of  successive  rotations  is  then  obtained  by  postmultiplication  of 
the  rotation  matrices  following  the  given  order  of  rotations,  as  in  (2.15).  With 
the  adopted  notation,  in  view  of  (2.5),  it  is 

R>  =  (R))-1  =  {R))t.  (2.16) 

Successive  rotations  can  be  also  specified  by  constantly  referring  them 
to  the  initial  frame;  in  this  case,  the  rotations  are  made  with  respect  to  a 
fixed,  frame.  Let  R®  be  the  rotation  matrix  of  frame  0-X\yiZ\  with  respect 
to  the  fixed  frame  O-Xoyozo-  Let  then  R2  denote  the  matrix  characterizing 
frame  0-x2y2z2  with  respect  to  Frame  0,  which  is  obtained  as  a  rotation  of 
Frame  1  according  to  the  matrix  R2.  Since  (2.15)  gives  a  composition  rule  of 
successive  rotations  about  the  axes  of  the  current  frame,  the  overall  rotation 
can  be  regarded  as  obtained  in  the  following  steps: 


(2.13) 

(2.14) 


1  Hereafter,  the  superscript  of  a  vector  or  a  matrix  denotes  the  frame  in  which  its 
components  are  expressed. 


2.3  Composition  of  Rotation  Matrices 


47 


•  First  realign  Frame  1  with  Frame  0  by  means  of  rotation  Hj. 

•  Then  make  the  rotation  expressed  by  R2  with  respect  to  the  current  frame. 

•  Finally  compensate  for  the  rotation  made  for  the  realignment  by  means  of 
the  inverse  rotation  R®. 

Since  the  above  rotations  are  described  with  respect  to  the  current  frame,  the 
application  of  the  composition  rule  (2.15)  yields 

_  no  pi  jM  pO 

-ft)  2  —  -fX^ -ZX  Q-ft^xX^. 

In  view  of  (2.16),  it  is 

R°2  =  R^Rl  (2.17) 

where  the  resulting  R2  is  different  from  the  matrix  R2  in  (2.15).  Hence,  it 
can  be  stated  that  composition  of  successive  rotations  with  respect  to  a  fixed 
frame  is  obtained  by  premultiplication  of  the  single  rotation  matrices  in  the 
order  of  the  given  sequence  of  rotations. 

By  recalling  the  meaning  of  a  rotation  matrix  in  terms  of  the  orientation 
of  a  current  frame  with  respect  to  a  fixed  frame,  it  can  be  recognized  that  its 
columns  are  the  direction  cosines  of  the  axes  of  the  current  frame  with  respect 
to  the  fixed  frame,  while  its  rows  (columns  of  its  transpose  and  inverse)  are 
the  direction  cosines  of  the  axes  of  the  fixed  frame  with  respect  to  the  current 
frame. 

An  important  issue  of  composition  of  rotations  is  that  the  matrix  product 
is  not  commutative.  In  view  of  this,  it  can  be  concluded  that  two  rotations 
in  general  do  not  commute  and  its  composition  depends  on  the  order  of  the 
single  rotations. 


Example  2.3 

Consider  an  object  and  a  frame  attached  to  it.  Figure  2.6  shows  the  effects  of  two 
successive  rotations  of  the  object  with  respect  to  the  current  frame  by  changing  the 
order  of  rotations.  It  is  evident  that  the  final  object  orientation  is  different  in  the  two 
cases.  Also  in  the  case  of  rotations  made  with  respect  to  the  current  frame,  the  final 
orientations  differ  (Fig.  2.7).  It  is  interesting  to  note  that  the  effects  of  the  sequence 
of  rotations  with  respect  to  the  fixed  frame  are  interchanged  with  the  effects  of  the 
sequence  of  rotations  with  respect  to  the  current  frame.  This  can  be  explained  by 
observing  that  the  order  of  rotations  in  the  fixed  frame  commutes  with  respect  to 
the  order  of  rotations  in  the  current  frame. 
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Fig.  2.7.  Successive  rotations  of  an  object  about  axes  of  fixed  frame 


2.4  Euler  Angles 

Rotation  matrices  give  a  redundant  description  of  frame  orientation;  in  fact, 
they  are  characterized  by  nine  elements  which  are  not  independent  but  related 
by  six  constraints  due  to  the  orthogonality  conditions  given  in  (2.4).  This  im¬ 
plies  that  three  parameters  are  sufficient  to  describe  orientation  of  a  rigid  body 


2.4  Euler  Angles 
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Fig.  2.8.  Representation  of  Euler  angles  ZYZ 


in  space.  A  representation  of  orientation  in  terms  of  three  independent  param¬ 
eters  constitutes  a  minimal  representation.  In  fact,  a  minimal  representation 
of  the  special  orthonormal  group  SO(m)  requires  m(m  —  l)/2  parameters; 
thus,  three  parameters  are  needed  to  parameterize  SO( 3),  whereas  only  one 
parameter  is  needed  for  a  planar  rotation  SO( 2). 

A  minimal  representation  of  orientation  can  be  obtained  by  using  a  set 
of  three  angles  (f>  =  [<p  it  ip]T .  Consider  the  rotation  matrix  expressing 
the  elementary  rotation  about  one  of  the  coordinate  axes  as  a  function  of  a 
single  angle.  Then,  a  generic  rotation  matrix  can  be  obtained  by  composing  a 
suitable  sequence  of  three  elementary  rotations  while  guaranteeing  that  two 
successive  rotations  are  not  made  about  parallel  axes.  This  implies  that  12 
distinct  sets  of  angles  are  allowed  out  of  all  27  possible  combinations;  each 
set  represents  a  triplet  of  Euler  angles.  In  the  following,  two  sets  of  Euler 
angles  are  analyzed;  namely,  the  ZYZ  angles  and  the  ZYX  (or  Roll-Pitch- 
Yaw)  angles. 


2.4.1  ZYZ  Angles 

The  rotation  described  by  ZYZ  angles  is  obtained  as  composition  of  the  fol¬ 
lowing  elementary  rotations  (Fig.  2.8): 

•  Rotate  the  reference  frame  by  the  angle  Lp  about  axis  z\  this  rotation  is 
described  by  the  matrix  Rz(ip)  which  is  formally  defined  in  (2.6). 

•  Rotate  the  current  frame  by  the  angle  $  about  axis  y'\  this  rotation  is 
described  by  the  matrix  Ry’ {{))  which  is  formally  defined  in  (2.7). 

•  Rotate  the  current  frame  by  the  angle  ip  about  axis  z" ;  this  rotation  is 
described  by  the  matrix  Rzn(ip )  which  is  again  formally  defined  in  (2.6). 
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The  resulting  frame  orientation  is  obtained  by  composition  of  rotations 
with  respect  to  current  frames,  and  then  it  can  be  computed  via  postmulti¬ 
plication  of  the  matrices  of  elementary  rotation,  i.e.,2 


R(4>)  =  Rz(y)Ry'(‘d)Rz»(4i) 

CcpC'dC'ij)  CipC'flS'ip  SipC'ip 

S (pCfiC'ip  +  CpStip  S (pCfiSsip  H-  C-tpC"^)  S^SrQ 

—  StfCip  StfS^p  Ctf 


(2.18) 


It  is  useful  to  solve  the  inverse  problem,  that  is  to  determine  the  set  of 
Euler  angles  corresponding  to  a  given  rotation  matrix 


r  11 

12 

r  13 

21 

r- 22 

f  23 

t*31 

Ai2 

f33 

Compare  this  expression  with  that  of  R{<fi)  in  (2.18).  By  considering  the 
elements  [1,3]  and  [2,3],  under  the  assumption  that  7*13  ^  0  and  r2 3  ^  0,  it 
follows  that 

ip  =  Atan2(r23,ri3) 

where  Atan2(y,a’)  is  the  arctangent  function  of  two  arguments3.  Then,  squar¬ 
ing  and  summing  the  elements  [1,3]  and  [2,3]  and  using  the  element  [3,3] 
yields 

$  =  At  an  2  ^  \jr\-:i  +  r|3,  r33 

The  choice  of  the  positive  sign  for  the  term  \J r23  +  r|3  limits  the  range  of 
feasible  values  of  7?  to  (0,7r).  On  this  assumption,  considering  the  elements 
[3, 1]  and  [3,  2]  gives 

4>  =  Atan2(r32,  -r3 1). 

In  sum,  the  requested  solution  is 


ip  =  Atan2(r23,ri3) 

7?  =  Atan2^^/r^~+ri3,r33^  (2.19) 

=  Atan2(r32,  -r3i). 

It  is  possible  to  derive  another  solution  which  produces  the  same  effects  as 
solution  (2.19).  Choosing  7?  in  the  range  (— it,  0)  leads  to 

<p  =  Atan2(— r23,  -ri3) 

2  The  notations  and  s<j,  are  the  abbreviations  for  cosf  and  sin^,  respectively; 
short-hand  notations  of  this  kind  will  be  adopted  often  throughout  the  text. 

3  The  function  Atan2(j/,  x)  computes  the  arctangent  of  the  ratio  y/x  but  utilizes  the 
sign  of  each  argument  to  determine  which  quadrant  the  resulting  angle  belongs 
to;  this  allows  the  correct  determination  of  an  angle  in  a  range  of  2ir. 
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Fig.  2.9.  Representation  of  Roll  Pitch-Yaw  angles 


i?  =  Atan2  +  rf3,  r33^  (2.20) 

ip  =  Atan2(-r32,r3i). 


Solutions  (2.19),  (2.20)  degenerate  when  s#  =  0;  in  this  case,  it  is  possible 
to  determine  only  the  sum  or  difference  of  (p  and  ip.  In  fact,  if  i)  =  0,7r, 
the  successive  rotations  of  ip  and  ip  are  made  about  axes  of  current  frames 
which  are  parallel,  thus  giving  equivalent  contributions  to  the  rotation;  see 
Problem  2. 2. 4 


2.4.2  RPY  Angles 

Another  set  of  Euler  angles  originates  from  a  representation  of  orientation  in 
the  (aero)nautical  field.  These  are  the  ZYX  angles,  also  called  Roll-Pitch- 
Yaw  angles,  to  denote  the  typical  changes  of  attitude  of  an  (air)craft.  In  this 
case,  the  angles  <p  =  [ip  ip]T  represent  rotations  defined  with  respect  to 
a  fixed  frame  attached  to  the  centre  of  mass  of  the  craft  (Fig.  2.9). 

The  rotation  resulting  from  Roll-Pitch-Yaw  angles  can  be  obtained  as 
follows: 

•  Rotate  the  reference  frame  by  the  angle  ip  about  axis  x  (yaw);  this  rotation 
is  described  by  the  matrix  Rx(ip)  which  is  formally  defined  in  (2.8). 

•  Rotate  the  reference  frame  by  the  angle  •&  about  axis  y  (pitch);  this  rotation 
is  described  by  the  matrix  Ry(i 9)  which  is  formally  defined  in  (2.7). 

•  Rotate  the  reference  frame  by  the  angle  ip  about  axis  z  (roll);  this  rotation 
is  described  by  the  matrix  Rz{jp)  which  is  formally  defined  in  (2.6). 


4  In  the  following  chapter,  it  will  be  seen  that  these  configurations  characterize  the 
so-called  representation  singularities  of  the  Euler  angles. 
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The  resulting  frame  orientation  is  obtained  by  composition  of  rotations  with 
respect  to  the  fixed  frame,  and  then  it  can  be  computed  via  premultiplication 
of  the  matrices  of  elementary  rotation,  i.e.,5 

R(4>)  =  Rz(^)RyWRx(if)  (2.21) 

CipC$  C(pS$Sijj  SipCijj  CipStfCip  SipSijj 

S(pC$  S tpStfSip  “I-  C^C^,  S ipSflC'tp 

CfiC’tjj 


As  for  the  Euler  angles  ZYZ,  the  inverse  solution  to  a  given  rotation  matrix 


rn 

ri2 

fi3 

R  = 

V21 

V22 

V23 

r3i 

r32 

T33 

can  be  obtained  by  comparing  it  with  the  expression  of  R((f)  in  (2.21).  The 
solution  for  d  in  the  range  (— 7r/2,7r/2)  is 


(fi  =  Atan2(r2i,  rn) 

'd  =  Atan2  r3i,  yj rf2  +  r|3^  (2.22) 

if  =  Atan2(r32,r33). 


The  other  equivalent  solution  for  d  in  the  range  (7t/2,  3n/2)  is 


ip  =  Atan2(— r2i,  -rn) 

d  =  Atan2^-r3i,  ~\Jrl2  +  rl^j  (2.23) 

if  =  Atan2(— r32,  -r33). 


Solutions  (2.22),  (2.23)  degenerate  when  c$  =  0;  in  this  case,  it  is  possible  to 
determine  only  the  sum  or  difference  of  <p  and  if. 


2.5  Angle  and  Axis 

A  nonminimal  representation  of  orientation  can  be  obtained  by  resorting  to 
four  parameters  expressing  a  rotation  of  a  given  angle  about  an  axis  in  space. 
This  can  be  advantageous  in  the  problem  of  trajectory  planning  for  a  manip¬ 
ulator’s  end-effector  orientation. 

Let  r  =  [rx  rv  rz]T  be  the  unit  vector  of  a  rotation  axis  with  respect 
to  the  reference  frame  O-xyz.  In  order  to  derive  the  rotation  matrix  R(id,  r) 
expressing  the  rotation  of  an  angle  id  about  axis  r,  it  is  convenient  to  compose 

5  The  ordered  sequence  of  rotations  XYZ  about  axes  of  the  fixed  frame  is  equivalent 
to  the  sequence  ZYX  about  axes  of  the  current  frame. 
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Fig.  2.10.  Rotation  of  an  angle  about  an  axis 


elementary  rotations  about  the  coordinate  axes  of  the  reference  frame.  The 
angle  is  taken  to  be  positive  if  the  rotation  is  made  counter-clockwise  about 
axis  r. 

As  shown  in  Fig.  2.10,  a  possible  solution  is  to  rotate  first  r  by  the  angles 
necessary  to  align  it  with  axis  z,  then  to  rotate  by  i9  about  z  and  finally 
to  rotate  by  the  angles  necessary  to  align  the  unit  vector  with  the  initial 
direction.  In  detail,  the  sequence  of  rotations,  to  be  made  always  with  respect 
to  axes  of  fixed  frame,  is  the  following: 

•  Align  r  with  z,  which  is  obtained  as  the  sequence  of  a  rotation  by  —a 
about  z  and  a  rotation  by  —0  about  y. 

•  Rotate  by  t?  about  z. 

•  Realign  with  the  initial  direction  of  r,  which  is  obtained  as  the  sequence 
of  a  rotation  by  0  about  y  and  a  rotation  by  a  about  z. 

In  sum,  the  resulting  rotation  matrix  is 

R(tf,r)  =  Rz(a)Ry(0)Rz(ti)Ry(-0)Rz(-a).  (2.24) 

From  the  components  of  the  unit  vector  r  it  is  possible  to  extract  the  tran¬ 
scendental  functions  needed  to  compute  the  rotation  matrix  in  (2.24),  so  as 
to  eliminate  the  dependence  from  a  and  /?;  in  fact,  it  is 

rv  rx 

sin  a  =  — .  cos  a  =  — . 

\]rl  +  rl  \jrl  +  rl 


sin/3  =  yj rl+rl 


cos  0  =  rz. 
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Then,  it  can  be  found  that  the  rotation  matrix  corresponding  to  a  given  angle 
and  axis  is  —  see  Problem  2.4  — 


R(0,r)  = 


rx(  1  -  eg)  +  c # 
rxry{  1  -  c&)  +  rzs$ 
rxrz(  1  -  ca)  -  ryStf 


rxr y(l  -  c$)  -  rzs # 
ry(l  -  o&)  + 
rvrz{  1  -  c^)  + 


rxrz(l 

V*(l 

rz(l 


For  this  matrix,  the  following  property  holds: 


Ctf)  T  t‘y-S',9 
C$)  rxS$  . 
Ctf)  +  C# 

(2.25) 


K(— d,  —r)  =  it(d,  r), 


(2.26) 


i.e.,  a  rotation  by  —  d  about  —  r  cannot  be  distinguished  from  a  rotation  by  $ 
about  r;  hence,  such  representation  is  not  unique. 

If  it  is  desired  to  solve  the  inverse  problem  to  compute  the  axis  and  angle 
corresponding  to  a  given  rotation  matrix 


r  n 

ri2 

r  i3 

R  = 

r2i 

r2  2 

r23 

r3i 

r3  2 

r33 

the  following  result  is  useful: 

q  -1  f  rn  +  r2  2  +  r33  -  1\ 

”  =  ( - 5 - ) 

r3 2  -  r23 
»T3  -  r3i  , 

^21  -  r-12 


1 

2  sin  d 


(2.27) 

(2.28) 


for  sintl  ^  0.  Notice  that  the  expressions  (2.27),  (2.28)  describe  the  rotation 
in  terms  of  four  parameters;  namely,  the  angle  and  the  three  components  of 
the  axis  unit  vector.  However,  it  can  be  observed  that  the  three  components 
of  r  are  not  independent  but  are  constrained  by  the  condition 


rl  +  r2y  +  r2z  =  1.  (2.29) 

If  sind  =  0,  the  expressions  (2.27),  (2.28)  become  meaningless.  To  solve  the 
inverse  problem,  it  is  necessary  to  directly  refer  to  the  particular  expressions 
attained  by  the  rotation  matrix  R  and  find  the  solving  formulae  in  the  two 
cases  d  =  0  and  d  =  n.  Notice  that,  when  d  =  0  (null  rotation),  the  unit 
vector  r  is  arbitrary  (singularity).  See  also  Problem  2.5. 


2.6  Unit  Quaternion 

The  drawbacks  of  the  angle/axis  representation  can  be  overcome  by  a  dif¬ 
ferent  four-parameter  representation;  namely,  the  unit  quaternion,  viz.  Euler 
parameters,  defined  as  Q  =  {rj,  e}  where: 


d 

V  =  cos  - 


(2.30) 
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■& 

e  =  sin  —  r;  (2-31) 

77  is  called  the  scalar  part  of  the  quaternion  while  e  =  [ex  ey  ez  }T  is  called 
the  vector  part  of  the  quaternion.  They  are  constrained  by  the  condition 

7?2  +  ex  +  ey  +  €1  =  1)  (2.32) 


hence,  the  name  unit  quaternion.  It  is  worth  remarking  that,  unlike  the  an¬ 
gle/axis  representation,  a  rotation  by  —  $  about  —  r  gives  the  same  quater¬ 
nion  as  that  associated  with  a  rotation  by  $  about  r;  this  solves  the  above 
nonuniqueness  problem.  In  view  of  (2.25),  (2.30),  (2.31),  (2.32),  the  rotation 
matrix  corresponding  to  a  given  quaternion  takes  on  the  form  —  see  Prob¬ 
lem  2.6 


R{V,  e) 


-2(r,*  +  el)-l 
2 [exey  T  pez) 

_  2(exez  -  r/ey) 


2(exey  -  r]ez) 

2(7?2  +  e2)  1 

2(eyez  rjex) 


2(exez  +  pev)  ' 
2{eycz  pex) 
2(ry2  +  e2)  —  1  _ 


(2.33) 


If  it  is  desired  to  solve  the  inverse  problem  to  compute  the  quaternion 
corresponding  to  a  given  rotation  matrix 


rn 

ri2 

n  3 

R  = 

r2 1 

r22 

T23 

U31 

7’32 

U33 

the  following  result  is  useful: 

1 


V  =  ^V^ii  +  r2  2  +  r33  +  1 


1 

6  ~  2 


sgn  (r3 2  -  r23)Vr n  -  ^22  -  r-33  +  1 
sgn  (ri3  -  r3i)A/r22  -  r33  -  J*n  +  1 
L  sgn  (r2i  -  ri2)  Vr33  -  -  r22  +  1 J 


(2.34) 

(2.35) 


where  conventionally  sgn  (x)  =  1  for  x  >  0  and  sgn  (x)  =  —  1  for  x  <  0.  Notice 
that  in  (2.34)  it  has  been  implicitly  assumed  ?y  >  0;  this  corresponds  to  an 
angle  $  £  [ — 7r,  7t]  ,  and  thus  any  rotation  can  be  described.  Also,  compared  to 
the  inverse  solution  in  (2.27),  (2.28)  for  the  angle  and  axis  representation,  no 
singularity  occurs  for  (2.34),  (2.35).  See  also  Problem  2.8. 

The  quaternion  extracted  from  R  1  =  RT  is  denoted  as  Q-1,  and  can  be 
computed  as 

QT1  =  {r),-e}.  (2.36) 

Let  Qi  =  {771 ,  ei}  and  Q2  =  {?y2,  e2}  denote  the  quaternions  corresponding 
to  the  rotation  matrices  R±  and  i?2,  respectively.  The  quaternion  correspond¬ 
ing  to  the  product  i?ii?2  is  given  by 

Qi  *  Q2  =  {mm  -  e{e2,me2  +  me  1  +  e1  x  e2}  (2.37) 


where  the  quaternion  product  operator  has  been  formally  introduced.  It  is 
easy  to  see  that  if  Q2  =  then  the  quaternion  {1, 0}  is  obtained  from  (2.37) 
which  is  the  identity  element  for  the  product.  See  also  Problem  2.9. 
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Fig.  2.11.  Representation  of  a  point  P  in  different  coordinate  frames 


2.7  Homogeneous  Transformations 

As  illustrated  at  the  beginning  of  the  chapter,  the  position  of  a  rigid  body  in 
space  is  expressed  in  terms  of  the  position  of  a  suitable  point  on  the  body  with 
respect  to  a  reference  frame  (translation),  while  its  orientation  is  expressed  in 
terms  of  the  components  of  the  unit  vectors  of  a  frame  attached  to  the  body 
—  with  origin  in  the  above  point  —  with  respect  to  the  same  reference  frame 
(rotation) . 

As  shown  in  Fig.  2.11,  consider  an  arbitrary  point  P  in  space.  Let  p° 
be  the  vector  of  coordinates  of  P  with  respect  to  the  reference  frame  O o~ 
XqUoZo-  Consider  then  another  frame  in  space  0\-X\y\Z\.  Let  o®  be  the  vector 
describing  the  origin  of  Frame  1  with  respect  to  Frame  0,  and  FI®  be  the 
rotation  matrix  of  Frame  1  with  respect  to  Frame  0.  Let  also  p 1  be  the  vector 
of  coordinates  of  P  with  respect  to  Frame  1.  On  the  basis  of  simple  geometry, 
the  position  of  point  P  with  respect  to  the  reference  frame  can  be  expressed 
as 

p°  =  o°1  +  R°1p1.  (2-38) 

Hence,  (2.38)  represents  the  coordinate  transformation  (translation  +  rota¬ 
tion)  of  a  bound  vector  between  two  frames. 

The  inverse  transformation  can  be  obtained  by  premultiplying  both  sides 
of  (2.38)  by  R?lT\  in  view  of(2.4),  it  follows  that 

p1  =  -R\to\  +  R°Tp°  (2.39) 

which,  via  (2.16),  can  be  written  as 

p1  =  - Rio ?  +  Rip0.  (2.40) 

In  order  to  achieve  a  compact  representation  of  the  relationship  between 
the  coordinates  of  the  same  point  in  two  different  frames,  the  homogeneous 
representation  of  a  generic  vector  p  can  be  introduced  as  the  vector  p  formed 
by  adding  a  fourth  unit  component,  i.e., 
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P  = 


(2.41) 


By  adopting  this  representation  for  the  vectors  p°  and  p 1  in  (2.38),  the  coor¬ 
dinate  transformation  can  be  written  in  terms  of  the  (4  x  4)  matrix 


A 


o 

l 


R°i 

oT  l . 


(2.42) 


which,  according  to  (2.41),  is  termed  homogeneous  transformation  matrix. 
Since  o°  £  1R3  e  R°  £  SO( 3),  this  matrix  belongs  to  the  special  Euclidean 
group  SE(3)  =  IR3  x  SO(3). 

As  can  be  easily  seen  from  (2.42),  the  transformation  of  a  vector  from 
Frame  1  to  Frame  0  is  expressed  by  a  single  matrix  containing  the  rotation 
matrix  of  Frame  1  with  respect  to  Frame  0  and  the  translation  vector  from 
the  origin  of  Frame  0  to  the  origin  of  Frame  l.6  Therefore,  the  coordinate 
transformation  (2.38)  can  be  compactly  rewritten  as 

P  =  A°p\  (2.43) 

The  coordinate  transformation  between  Frame  0  and  Frame  1  is  described 
by  the  homogeneous  transformation  matrix  A J  which  satisfies  the  equation 

P1  =  ASp°=(A5)-1p0.  (2.44) 

This  matrix  is  expressed  in  a  block-partitioned  form  as 


A1  - 

R °T 

Ro 

-Rio0, 

Ao  — 

i 

O 

i 

.  oT 

l  . 

which  gives  the  homogeneous  representation  form  of  the  result  already  estab¬ 
lished  by  (2.39),  (2.40)  —  see  Problem  2.10. 

Notice  that  for  the  homogeneous  transformation  matrix  the  orthogonality 
property  does  not  hold;  hence,  in  general, 

A'1  yf  At.  (2.46) 

In  sum,  a  homogeneous  transformation  matrix  expresses  the  coordinate 
transformation  between  two  frames  in  a  compact  form.  If  the  frames  have  the 


It  can  be  shown  that  in  (2.42)  non-null  values  of  the  first  three  elements  of  the 
fourth  row  of  A  produce  a  perspective  effect,  while  values  other  than  unity  for 
the  fourth  element  give  a  scaling  effect. 
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Fig.  2.12.  Conventional  representations  of  joints 


same  origin,  it  reduces  to  the  rotation  matrix  previously  defined.  Instead,  if 
the  frames  have  distinct  origins,  it  allows  the  notation  with  superscripts  and 
subscripts  to  be  kept  which  directly  characterize  the  current  frame  and  the 
fixed  frame. 

Analogously  to  what  presented  for  the  rotation  matrices,  it  is  easy  to 
verify  that  a  sequence  of  coordinate  transformations  can  be  composed  by  the 
product 

p°  =  A\A\...An~1vn  (2.47) 

where  A*- 1  denotes  the  homogeneous  transformation  relating  the  description 
of  a  point  in  Frame  i  to  the  description  of  the  same  point  in  Frame  i  —  1. 


2.8  Direct  Kinematics 

A  manipulator  consists  of  a  series  of  rigid  bodies  (links)  connected  by  means  of 
kinematic  pairs  or  joints.  Joints  can  be  essentially  of  two  types:  revolute  and 
prismatic;  conventional  representations  of  the  two  types  of  joints  are  sketched 
in  Fig.  2.12.  The  whole  structure  forms  a  kinematic  chain.  One  end  of  the 
chain  is  constrained  to  a  base.  An  end-effector  (gripper,  tool)  is  connected  to 
the  other  end  allowing  manipulation  of  objects  in  space. 

From  a  topological  viewpoint,  the  kinematic  chain  is  termed  open  when 
there  is  only  one  sequence  of  links  connecting  the  two  ends  of  the  chain.  Al¬ 
ternatively,  a  manipulator  contains  a  closed  kinematic  chain  when  a  sequence 
of  links  forms  a  loop. 

The  mechanical  structure  of  a  manipulator  is  characterized  by  a  number  of 
degrees  of  freedom  (DOFs)  which  uniquely  determine  its  posture.'7  Each  DOF 
is  typically  associated  with  a  joint  articulation  and  constitutes  a.  joint  variable. 
The  aim  of  direct  kinematics  is  to  compute  the  pose  of  the  end-effector  as  a 
function  of  the  joint  variables. 


'  The  term  posture  of  a  kinematic  chain  denotes  the  pose  of  all  the  rigid  bodies 
composing  the  chain.  Whenever  the  kinematic  chain  reduces  to  a  single  rigid 
body,  then  the  posture  coincides  with  the  pose  of  the  body. 
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Fig.  2.13.  Description  of  the  position  and  orientation  of  the  end-effector  frame 


It  was  previously  illustrated  that  the  pose  of  a  body  with  respect  to  a 
reference  frame  is  described  by  the  position  vector  of  the  origin  and  the  unit 
vectors  of  a  frame  attached  to  the  body.  Hence,  with  respect  to  a  reference 
frame  Ob-XbUbZb,  the  direct  kinematics  function  is  expressed  by  the  homoge¬ 
neous  transformation  matrix 


Tbe(q) 


nbe(q )  sbe(q)  ab(q)  pb{q) 

.  0  0  0  1  . 


(2.48) 


where  q  is  the  ( n  x  1)  vector  of  joint  variables,  ne,  se,  ae  are  the  unit  vectors 
of  a  frame  attached  to  the  end-effector,  and  pe  is  the  position  vector  of  the 
origin  of  such  a  frame  with  respect  to  the  origin  of  the  base  frame  Ob-XbUbZb 
(Fig.  2.13).  Note  that  ne,  se,  ae  and  pe  are  a  function  of  q. 

The  frame  Ob-XbVbZb  is  termed  base  frame.  The  frame  attached  to  the  end- 
effector  is  termed  end- effector  frame  and  is  conveniently  chosen  according  to 
the  particular  task  geometry.  If  the  end-effector  is  a  gripper,  the  origin  of  the 
end-effector  frame  is  located  at  the  centre  of  the  gripper,  the  unit  vector  ae 
is  chosen  in  the  approach  direction  to  the  object,  the  unit  vector  se  is  chosen 
normal  to  ae  in  the  sliding  plane  of  the  jaws,  and  the  unit  vector  ne  is  chosen 
normal  to  the  other  two  so  that  the  frame  (ne,  se,ae)  is  right-handed. 

A  first  way  to  compute  direct  kinematics  is  offered  by  a  geometric  analysis 
of  the  structure  of  the  given  manipulator. 


60 


2  Kinematics 


Example  2.4 

Consider  the  two-link  planar  arm  in  Fig.  2.14.  On  the  basis  of  simple  trigonometry, 
the  choice  of  the  joint  variables,  the  base  frame,  and  the  end-effector  frame  leads 
to8 


'0 

Sl2 

Cl2 

aiCi  +  &2C12 

n\  sbe  ab  pbe 

0 

-C12 

S12 

aisi  +  a2Si2 

1 

0 

0 

0 

_  0  0  0  1  _ 

0 

0 

0 

1 

(2.49) 


It  is  not  difficult  to  infer  that  the  effectiveness  of  a  geometric  approach 
to  the  direct  kinematics  problem  is  based  first  on  a  convenient  choice  of  the 
relevant  quantities  and  then  on  the  ability  and  geometric  intuition  of  the  prob¬ 
lem  solver.  Whenever  the  manipulator  structure  is  complex  and  the  number  of 
joints  increases,  it  is  preferable  to  adopt  a  less  direct  solution,  which,  though, 
is  based  on  a  systematic,  general  procedure.  The  problem  becomes  even  more 
complex  when  the  manipulator  contains  one  or  more  closed  kinematic  chains. 
In  such  a  case,  as  it  will  be  discussed  later,  there  is  no  guarantee  to  obtain  an 
analytical  expression  for  the  direct  kinematics  function  in  (2.48). 

2.8.1  Open  Chain 

Consider  an  open-chain  manipulator  constituted  by  n  +  1  links  connected  by 
n  joints,  where  Link  0  is  conventionally  fixed  to  the  ground.  It  is  assumed  that 
each  joint  provides  the  mechanical  structure  with  a  single  DOF,  corresponding 
to  the  joint  variable. 

The  construction  of  an  operating  procedure  for  the  computation  of  di¬ 
rect  kinematics  is  naturally  derived  from  the  typical  open  kinematic  chain  of 
the  manipulator  structure.  In  fact,  since  each  joint  connects  two  consecutive 


The  notations  d...j  denote  respectively  sin  {qi  +  ■  ■  -  +  qj),  cos  (qt  +  . . .  +  qj). 
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Fig.  2.15.  Coordinate  transformations  in  an  open  kinematic  chain 


links,  it  is  reasonable  to  consider  first  the  description  of  kinematic  relationship 
between  consecutive  links  and  then  to  obtain  the  overall  description  of  manip¬ 
ulator  kinematics  in  a  recursive  fashion.  To  this  purpose,  it  is  worth  defining 
a  coordinate  frame  attached  to  each  link,  from  Link  0  to  Link  n.  Then,  the 
coordinate  transformation  describing  the  position  and  orientation  of  Frame  n 
with  respect  to  Frame  0  (Fig.  2.15)  is  given  by 

T0n(q)  =  A01(qi)A12(q2)...A^-1(qn).  (2.50) 

As  requested,  the  computation  of  the  direct  kinematics  function  is  recursive 
and  is  obtained  in  a  systematic  manner  by  simple  products  of  the  homogeneous 
transformation  matrices  A*-1(g j)  (for  i  =  1, . . . ,  n),  each  of  which  is  a  function 
of  a  single  joint  variable. 

With  reference  to  the  direct  kinematics  equation  in  (2.49),  the  actual  co¬ 
ordinate  transformation  describing  the  position  and  orientation  of  the  end- 
effector  frame  with  respect  to  the  base  frame  can  be  obtained  as 

Tbe{q)  =  Tb0T°n{q)Tne  (2.51) 

where  T q  and  T”  are  two  (typically)  constant  homogeneous  transformations 
describing  the  position  and  orientation  of  Frame  0  with  respect  to  the  base 
frame,  and  of  the  end-effector  frame  with  respect  to  Frame  n,  respectively. 

2.8.2  Denavit— Hartenberg  Convention 

In  order  to  compute  the  direct  kinematics  equation  for  an  open-chain  manip¬ 
ulator  according  to  the  recursive  expression  in  (2.50),  a  systematic,  general 
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JOINT  i- 1  JOINT  i  JOINT  i  +  1 


Fig.  2.16.  Denavit  Hartenberg  kinematic  parameters 


method  is  to  be  derived  to  define  the  relative  position  and  orientation  of  two 
consecutive  links;  the  problem  is  that  of  determining  two  frames  attached  to 
the  two  links  and  computing  the  coordinate  transformations  between  them. 
In  general,  the  frames  can  be  arbitrarily  chosen  as  long  as  they  are  attached 
to  the  link  they  are  referred  to.  Nevertheless,  it  is  convenient  to  set  some  rules 
also  for  the  definition  of  the  link  frames. 

With  reference  to  Fig.  2.16,  let  Axis  i  denote  the  axis  of  the  joint  connect¬ 
ing  Link  *  —  1  to  Link  i:  the  so-called  Denavit-Hartenberg  convention  (DH)  is 
adopted  to  define  link  Frame  i: 

•  Choose  axis  Zi  along  the  axis  of  Joint  i+1. 

•  Locate  the  origin  O,  at  the  intersection  of  axis  Z{  with  the  common  normal9 
to  axes  Zi- \  and  Z{.  Also,  locate  O,/  at  the  intersection  of  the  common 
normal  with  axis  2j_;i . 

•  Choose  axis  x.t  along  the  common  normal  to  axes  Zj_i  and  Z{  with  positive 
direction  from  Joint  i  to  Joint  i+1. 

•  Choose  axis  yi  so  as  to  complete  a  right-handed  frame. 

The  Denavit-Hartenberg  convention  gives  a  nonunique  definition  of  the  link 
frame  in  the  following  cases: 

•  For  Frame  0,  only  the  direction  of  axis  Zq  is  specified;  then  Oq  and  Xq  can 
be  arbitrarily  chosen. 

•  For  Frame  n,  since  there  is  no  Joint  n  + 1,  zn  is  not  uniquely  defined  while 
xn  has  to  be  normal  to  axis  zn-\.  Typically,  Joint  n  is  revolute,  and  thus 
zn  can  be  aligned  with  the  direction  of  zn-\. 

9  The  common  normal  between  two  lines  is  the  line  containing  the  minimum  dis¬ 
tance  segment  between  the  two  lines. 
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•  When  two  consecutive  axes  are  parallel,  the  common  normal  between  them 
is  not  uniquely  defined. 

•  When  two  consecutive  axes  intersect,  the  positive  direction  of  Xi  is  arbi¬ 
trary. 

•  When  Joint  i  is  prismatic,  only  the  direction  of  Zi-\  is  specified. 

In  all  such  cases,  the  indeterminacy  can  be  exploited  to  simplify  the  procedure; 
for  instance,  the  axes  of  consecutive  frames  can  be  made  parallel. 

Once  the  link  frames  have  been  established,  the  position  and  orientation  of 
Frame  i  with  respect  to  Frame  i  —  1  are  completely  specified  by  the  following 
parameters : 

ai  distance  between  O,  and  CO, 
di  coordinate  of  CO  along  Zi-i, 

ai  angle  between  axes  Zi-\  and  Zi  about  axis  Xi  to  be  taken  positive  when 
rotation  is  made  counter-clockwise, 

di  angle  between  axes  Xi-i  and  Xi  about  axis  Zi-\  to  be  taken  positive  when 
rotation  is  made  counter-clockwise. 

Two  of  the  four  parameters  (a,j  and  a j)  are  always  constant  and  depend 
only  on  the  geometry  of  connection  between  consecutive  joints  established 
by  Link  i.  Of  the  remaining  two  parameters,  only  one  is  variable  depending 
on  the  type  of  joint  that  connects  Link  i  —  1  to  Link  i.  In  particular: 

•  if  Joint  i  is  revolute  the  variable  is  di, 

•  if  Joint  i  is  prismatic  the  variable  is  di. 

At  this  point,  it  is  possible  to  express  the  coordinate  transformation  between 
Frame  i  and  Frame  i  —  1  according  to  the  following  steps: 

•  Choose  a  frame  aligned  with  Frame  i  —  1. 

•  Translate  the  chosen  frame  by  di  along  axis  Zi-\  and  rotate  it  by  di  about 
axis  Zi- 1;  this  sequence  aligns  the  current  frame  with  Frame  i!  and  is 
described  by  the  homogeneous  transformation  matrix 

ci?i  —  sni  0  0 

Stft  c^i  0  0 

0*  0*  1  di  ' 

0  0  0  1  . 

•  Translate  the  frame  aligned  with  Frame  i'  by  ai  along  axis  Xjj  and  rotate 
it  by  ai  about  axis  x ,/ ;  this  sequence  aligns  the  current  frame  with  Frame  i 
and  is  described  by  the  homogeneous  transformation  matrix 

0  ai' 

~S(Xi  0 
O-oti  0 

0  1  . 


-1  0 

0  C-Oii 

0  So., 
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•  The  resulting  coordinate  transformation  is  obtained  by  postmultiplication 
of  the  single  transformations  as 


-c* 

Sfii  Ccti 

s#isai 

OjiC"di 

s#i 

0 

C"&i  Coti 

Cfii  Soti 
CcsLi 

CliStfi 

di 

(2.52) 

.  0 

0 

0 

1  . 

Notice  that  the  transformation  matrix  from  Frame  i  to  Frame  i— 1  is  a  function 
only  of  the  joint  variable  qi,  that  is,  d*  for  a  revolute  joint  or  di  for  a  prismatic 
joint. 

To  summarize,  the  Denavit-Hartenberg  convention  allows  the  construction 
of  the  direct  kinematics  function  by  composition  of  the  individual  coordinate 
transformations  expressed  by  (2.52)  into  one  homogeneous  transformation 
matrix  as  in  (2.50).  The  procedure  can  be  applied  to  any  open  kinematic 
chain  and  can  be  easily  rewritten  in  an  operating  form  as  follows. 

1.  Find  and  number  consecutively  the  joint  axes;  set  the  directions  of  axes 

)  *  •  •  ?  Zn—  1- 

2.  Choose  Frame  0  by  locating  the  origin  on  axis  Zo ;  axes  Xo  and  yo  are 
chosen  so  as  to  obtain  a  right-handed  frame.  If  feasible,  it  is  worth  choosing 
Frame  0  to  coincide  with  the  base  frame. 

Execute  steps  from  3  to  5  for  i  =  1, . . . ,  n  —  1: 

3.  Locate  the  origin  Oi  at  the  intersection  of  Zi  with  the  common  normal  to 
axes  z^ i  and  Zi .  If  axes  z,_ i  and  Zi  are  parallel  and  Joint  i  is  revolute, 
then  locate  Oi  so  that  di  =  0;  if  Joint  i  is  prismatic,  locate  Oi  at  a  reference 
position  for  the  joint  range,  e.g.,  a  mechanical  limit. 

4.  Choose  axis  Xi  along  the  common  normal  to  axes  Zi-\  and  Zi  with  direction 
from  Joint  i  to  Joint  i  +  1. 

5.  Choose  axis  y,;  so  as  to  obtain  a  right-handed  frame. 

To  complete: 

6.  Choose  Frame  n;  if  Joint  n  is  revolute,  then  align  zn  with  zn_i,  otherwise, 
if  Joint  n  is  prismatic,  then  choose  zn  arbitrarily.  Axis  xn  is  set  according 
to  step  4. 

7.  For  i  =  1 , ,n,  form  the  table  of  parameters  ai,  di,  ai,  dj. 

8.  On  the  basis  of  the  parameters  in  7,  compute  the  homogeneous  transfor¬ 
mation  matrices  A*_1(<7,;)  for  *  =  1, . . . ,  n. 

9.  Compute  the  homogeneous  transformation  T°n{q)  =  A° . . .  A™-1  that 
yields  the  position  and  orientation  of  Frame  n  with  respect  to  Frame  0. 

10.  Given  Tq  and  T”,  compute  the  direct  kinematics  function  as  Tbe(q)  = 
TqT°  T"  that  yields  the  position  and  orientation  of  the  end-effector  frame 
with  respect  to  the  base  frame. 
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Fig.  2.17.  Connection  of  a  single  link  in  the  chain  with  two  links 


For  what  concerns  the  computational  aspects  of  direct  kinematics,  it  can  be 
recognized  that  the  heaviest  load  derives  from  the  evaluation  of  transcenden¬ 
tal  functions.  On  the  other  hand,  by  suitably  factorizing  the  transformation 
equations  and  introducing  local  variables,  the  number  of  flops  (additions  + 
multiplications)  can  be  reduced.  Finally,  for  computation  of  orientation  it  is 
convenient  to  evaluate  the  two  unit  vectors  of  the  end-effector  frame  of  sim¬ 
plest  expression  and  derive  the  third  one  by  vector  product  of  the  first  two. 


2.8.3  Closed  Chain 

The  above  direct  kinematics  method  based  on  the  DH  convention  exploits 
the  inherently  recursive  feature  of  an  open-chain  manipulator.  Nevertheless, 
the  method  can  be  extended  to  the  case  of  manipulators  containing  closed 
kinematic  chains  according  to  the  technique  illustrated  below. 

Consider  a  closed-chain  manipulator  constituted  by  n  +  1  links.  Because 
of  the  presence  of  a  loop,  the  number  of  joints  l  must  be  greater  than  re.  in 
particular,  it  can  be  understood  that  the  number  of  closed  loops  is  equal  to 
l  —  n. 

With  reference  to  Fig.  2.17,  Links  0  through  i  are  connected  successively 
through  the  first  i  joints  as  in  an  open  kinematic  chain.  Then,  Joint  i  +  V 
connects  Link  i  with  Link  i  +  1'  while  Joint  i  +  V  connects  Link  i  with 
Link  i  +  1";  the  axes  of  Joints  i  +  V  and  i  +  1"  are  assumed  to  be  aligned. 
Although  not  represented  in  the  figure,  Links  i  +  V  and  i  +  are  members 
of  the  closed  kinematic  chain.  In  particular,  Link  i  +  V  is  further  connected 
to  Link  i  +  2'  via  Joint  i  +  2'  and  so  forth,  until  Link  j  via  Joint  j.  Likewise, 
Link  i  +  1"  is  further  connected  to  Link  i  +  2"  via  Joint  i  +  2"  and  so  forth, 
until  Link  k  via  Joint  k.  Finally,  Links  j  and  k  are  connected  together  at 
Joint  j  +  1  to  form  a  closed  chain.  In  general,  j  ^  k. 

In  order  to  attach  frames  to  the  various  links  and  apply  DH  convention, 
one  closed  kinematic  chain  is  taken  into  account.  The  closed  chain  can  be 
virtually  cut  open  at  Joint  j  +  1,  i.e. ,  the  joint  between  Link  j  and  Link  k. 
An  equivalent  tree-structured  open  kinematic  chain  is  obtained,  and  thus  link 
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Fig.  2.18.  Coordinate  transformations  in  a  closed  kinematic  chain 


frames  can  be  defined  as  in  Fig.  2.18.  Since  Links  0  through  i  occur  before 
the  two  branches  of  the  tree,  they  are  left  out  of  the  analysis.  For  the  same 
reason,  Links  j  +  1  through  n  are  left  out  as  well.  Notice  that  Frame  i  is  to 
be  chosen  with  axis  Zi  aligned  with  the  axes  of  Joints  i  +  V  and  i  +  1". 

It  follows  that  the  position  and  orientation  of  Frame  j  with  respect  to 
Frame  i  can  be  expressed  by  composing  the  homogeneous  transformations  as 

=  At+V(qi+1,) . . .  Af  \qj)  (2.53) 

where  q'  =  [ qt^v  ...  Qj]T-  Likewise,  the  position  and  orientation  of 
Frame  k  with  respect  to  Frame  i  is  given  by 

Ai(q")  =  A;:+1„fe+1") . . .  Akk-\qk)  (2.54) 

where  q"  =  [  qi+v  ...  qk]T  ■ 

Since  Links  j  and  k  are  connected  to  each  other  through  Joint  j  +  1, 
it  is  worth  analyzing  the  mutual  position  and  orientation  between  Frames  j 
and  k ,  as  illustrated  in  Fig.  2.19.  Notice  that,  since  Links  j  and  k  are  connected 
to  form  a  closed  chain,  axes  Zj  and  zk  are  aligned.  Therefore,  the  following 
orientation  constraint  has  to  be  imposed  between  Frames  j  and  k: 

z){q')  =  zk(<l"),  (2-55) 

where  the  unit  vectors  of  the  two  axes  have  been  conveniently  referred  to 
Frame  i. 

Moreover,  if  Joint  j  + 1  is  prismatic,  the  angle  i)jk  between  axes  Xj  and  xk 
is  fixed;  hence,  in  addition  to  (2.55),  the  following  constraint  is  obtained: 

xf  (q')xl(q")  =  costijk.  (2.56) 

Obviously,  there  is  no  need  to  impose  a  similar  constraint  on  axes  yj  and  yk 
since  that  would  be  redundant. 
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Fig.  2.19.  Coordinate  transformation  at  the  cut  joint 


Regarding  the  position  constraint  between  Frames  j  and  k,  let  p*  and 
plk  respectively  denote  the  positions  of  the  origins  of  Frames  j  and  fc,  when 
referred  to  Frame  i.  By  projecting  on  Frame  j  the  distance  vector  of  the  origin 
of  Frame  k  from  Frame  j,  the  following  constraint  has  to  be  imposed: 

Ri(q')(p)(q')-ptiq"))  =  [  0  0  djk]T  (2.57) 


where  Rj  =  i?f  denotes  the  orientation  of  Frame  i  with  respect  to  Frame  j. 
At  this  point,  if  Joint  j  +  1  is  revolute,  then  djk  is  a  fixed  offset  along  axis  Zj\ 
hence,  the  three  equalities  of  (2.57)  fully  describe  the  position  constraint.  If, 
however,  Joint  j  +  1  is  prismatic,  then  djk  varies.  Consequently,  only  the  first 
two  equalities  of  (2.57)  describe  the  position  constraint,  i.e. , 
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where  i?*  =  [®*. 
In  summary. 


if  Joint  j  +  1  is  revolute  the  constraints  are 

(Ri(q')(pi(q')-pi(q"))  =  [  0  0  djk]T 
\ z)(q')=zi(q "), 


whereas  if  Joint  j  +  1  is  prismatic  the  constraints  are 
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In  either  case,  there  are  six  equalities  that  must  be  satisfied.  Those  should  be 
solved  for  a  reduced  number  of  independent  joint  variables  to  be  keenly  chosen 
among  the  components  of  q'  and  q"  which  characterize  the  DOFs  of  the  closed 
chain.  These  are  the  natural  candidates  to  be  the  actuated  joints,  while  the 
other  joints  in  the  chain  (including  the  cut  joint)  are  typically  not  actuated. 
Such  independent  variables,  together  with  the  remaining  joint  variables  not 
involved  in  the  above  analysis,  constitute  the  joint  vector  q  that  allows  the 
direct  kinematics  equation  to  be  computed  as 

T°n(q)  =  A';A\Ai.  (2.61) 

where  the  sequence  of  successive  transformations  after  the  closure  of  the  chain 
has  been  conventionally  resumed  from  Frame  j. 

In  general,  there  is  no  guarantee  to  solve  the  constraints  in  closed  form 
unless  the  manipulator  has  a  simple  kinematic  structure.  In  other  words,  for 
a  given  manipulator  with  a  specific  geometry,  e.g.,  a  planar  structure,  some  of 
the  above  equalities  may  become  dependent.  Hence,  the  number  of  indepen¬ 
dent  equalities  is  less  than  six  and  it  should  likely  be  easier  to  solve  them. 

To  conclude,  it  is  worth  sketching  the  operating  form  of  the  procedure  to 
compute  the  direct  kinematics  function  for  a  closed-chain  manipulator  using 
the  Denavit-Hartenberg  convention. 

1.  In  the  closed  chain,  select  one  joint  that  is  not  actuated.  Assume  that  the 
joint  is  cut  open  so  as  to  obtain  an  open  chain  in  a  tree  structure. 

2.  Compute  the  homogeneous  transformations  according  to  DH  convention. 

3.  Find  the  equality  constraints  for  the  two  frames  connected  by  the  cut  joint. 

4.  Solve  the  constraints  for  a  reduced  number  of  joint  variables. 

5.  Express  the  homogeneous  transformations  in  terms  of  the  above  joint  vari¬ 
ables  and  compute  the  direct  kinematics  function  by  composing  the  various 
transformations  from  the  base  frame  to  the  end-effector  frame. 


2.9  Kinematics  of  Typical  Manipulator  Structures 

This  section  contains  several  examples  of  computation  of  the  direct  kinemat¬ 
ics  function  for  typical  manipulator  structures  that  are  often  encountered  in 
industrial  robots. 

With  reference  to  the  schematic  representation  of  the  kinematic  chain, 
manipulators  are  usually  illustrated  in  postures  where  the  joint  variables,  de¬ 
fined  according  to  the  DH  convention,  are  different  from  zero;  such  values 
might  differ  from  the  null  references  utilized  for  robot  manipulator  program¬ 
ming.  Hence,  it  will  be  necessary  to  sum  constant  contributions  (offsets)  to 
the  values  of  the  joint  variables  measured  by  the  robot  sensory  system,  so  as 
to  match  the  references. 
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Fig.  2.20.  Three-link  planar  arm 


2.9.1  Three-link  Planar  Arm 

Consider  the  three-link  planar  arm  in  Fig.  2.20,  where  the  link  frames  have 
been  illustrated.  Since  the  revolute  axes  are  all  parallel,  the  simplest  choice 
was  made  for  all  axes  x,  along  the  direction  of  the  relative  links  (the  direction 
of  xq  is  arbitrary)  and  all  lying  in  the  plane  (xo,yo)-  In  this  way,  all  the 
parameters  d,;  are  null  and  the  angles  between  the  axes  x,  directly  provide  the 
joint  variables.  The  DH  parameters  are  specified  in  Table  2.1. 


Table  2.1.  DH  parameters  for  the  three-link  planar  arm 


Link 

Cli 

ai 

di 

di 

1 

ai 

0 

0 

d! 

2 

02 

0 

0 

d  2 

3 

03 

0 

0 

1?3 

Since  all  joints  are  revolute,  the  homogeneous  transformation  matrix  de¬ 
fined  in  (2.52)  has  the  same  structure  for  each  joint,  i.e., 

Ci  s  j  0  r/.y  c,j 

Si  Ci  0  CliSi 

0  0  10 

.0001. 


Ar1^) 


i  =  1,2,3. 


(2.62) 
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Fig.  2.21.  Parallelogram  arm 


Computation  of  the  direct  kinematics  function  as  in  (2.50)  yields 


T°3(q)  =  A^AlAl 


"Ci23  ^123  0  OiCi  +  CI2C12  +  CI3C123 

Sl23  Cl23  0  CI1S1  +  02Si2  +  03S123 

0  0  1  0 

.  0  0  0  1 


(2.63) 


where  q  =  I'd  1  $2  $ 3  }T  ■  Notice  that  the  unit  vector  z°  of  Frame  3  is  aligned 

with  zo  =  [0  0  l]r,  in  view  of  the  fact  that  all  revolute  joints  are  parallel 

to  axis  Zq.  Obviously,  pz  =  0  and  all  three  joints  concur  to  determine  the 
end-effector  position  in  the  plane  of  the  structure.  It  is  worth  pointing  out 
that  Frame  3  does  not  coincide  with  the  end-effector  frame  (Fig.  2.13),  since 
the  resulting  approach  unit  vector  is  aligned  with  a?®  and  not  with  z\.  Thus, 
assuming  that  the  two  frames  have  the  same  origin,  the  constant  transforma¬ 
tion 


-  0  010- 
0  10  0 
-10  0  0 
.  0  0  0  1. 


is  needed,  having  taken  n  aligned  with  Zg . 


2.9.2  Parallelogram  Arm 

Consider  the  parallelogram  arm  in  Fig.  2.21.  A  closed  chain  occurs  where  the 
first  two  joints  connect  Link  V  and  Link  1"  to  Link  0,  respectively.  Joint  4  was 
selected  as  the  cut  joint,  and  the  link  frames  have  been  established  accordingly. 
The  DH  parameters  are  specified  in  Table  2.2,  where  ay  =  ay  and  ay  =  ay 
in  view  of  the  parallelogram  structure. 

Notice  that  the  parameters  for  Link  4  are  all  constant.  Since  the  joints 
are  revolute,  the  homogeneous  transformation  matrix  defined  in  (2.52)  has 
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Table  2.2.  DH  parameters  for  the  parallelogram  arm 


Link 

CLi 

OLi 

di 

fli 

T 

ay 

0 

0 

fly 

2' 

ay 

0 

0 

fly 

3' 

a3f 

0 

0 

fly 

1" 

ay 

0 

0 

fly 

4 

a4 

0 

0 

0 

the  same  structure  for  each  joint,  i.e.,  as  in  (2.62)  for  Joints  1',  2',  3'  and  1". 
Therefore,  the  coordinate  transformations  for  the  two  branches  of  the  tree  are 
respectively: 


-4-3'  W)  —  Ay  Ay  Ay 


’Cl'2'3' 

Sl'2'3' 

0 

_  0 


-Si/2'3'  0  avci>  +  a2'Cp2'  +  a3'Ci'2'3' 

Cl'2'3'  0  ai'Si'  +  a2'Si'2'  +  a3'Si'2'3' 

0  1  0 

0  0  1 


where  q '  =  [$i'  fl2'  $3'  ]T,  and 


"  Cy  —Sy  0  ap/Ci"' 

Sy  Cy  0  ai"Si" 

0  0  10 

.  0  0  0  1  . 


where  q"  =  fly.  To  complete,  the  constant  homogeneous  transformation  for 
the  last  link  is 


A 


3' 

4 


■  1  0  0  a4- 

0  10  0 

0  0  10 

.0  0  0  1  . 


With  reference  to  (2.59),  the  position  constraints  are  {dyy  =  0) 


RtW)  (plW)-pUd")) 


o 

o 

0 


while  the  orientation  constraints  are  satisfied  independently  of  q'  and  q" .  Since 
ay  =  ay  and  ay  =  ay,  two  independent  constraints  can  be  extracted,  i.e., 


Ol'(cl'  +  Cl'2'3')  +  ay  (ci'2'  —  Cy)  —  0 
ay(sy  +  Si'2'3')  +  ay{syy  ~  Si")  =  0. 

In  order  to  satisfy  them  for  any  choice  of  ay  and  ai",  it  must  be 


fly  =  Ay  -  Ai1 

$3'  =7 T  —  fly  =  7T  —  fly  +  fly 
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Therefore,  the  vector  of  joint  variables  is  q  =  •d\"]r .  These  joints  are 

natural  candidates  to  be  the  actuated  joints.10  Substituting  the  expressions 
of  d2>  and  $3'  into  the  homogeneous  transformation  A°,  and  computing  the 
direct  kinematics  function  as  in  (2.61)  yields 

— On  si'  0  ayCy  —  U4C1' " 

s  1 1  Ci/  0  S\"  U4S1' 

0  0  1  0  •  1  j 

0  0  0  1 

A  comparison  between  (2.64)  and  (2.49)  reveals  that  the  parallelogram  arm  is 
kinematically  equivalent  to  a  two-link  planar  arm.  The  noticeable  difference, 
though,  is  that  the  two  actuated  joints  —  providing  the  DOFs  of  the  structure 
are  located  at  the  base.  This  will  greatly  simplify  the  dynamic  model  of 
the  structure,  as  will  be  seen  in  Sect.  7.3.3. 


T°4(q)  =  A°3,(q)A*  = 


2.9.3  Spherical  Arm 

Consider  the  spherical  arm  in  Fig.  2.22,  where  the  link  frames  have  been 
illustrated.  Notice  that  the  origin  of  Frame  0  was  located  at  the  intersection 
of  zq  with  z\  so  that  d\  =  0;  analogously,  the  origin  of  Frame  2  was  located 
at  the  intersection  between  Z\  and  z2.  The  DH  parameters  are  specified  in 
Table  2.3. 


Table  2.3.  DH  parameters  for  the  spherical  arm 


Link 

ai 

at 

di 

di 

1 

0 

— 7r/2 

0 

di 

2 

0 

7t/2 

d2 

d2 

3 

0 

0 

0 

The  homogeneous  transformation  matrices  defined  in  (2.52)  are  for  the 
single  joints: 


'Cl 

0 

-Si 

0- 

-C2 

0 

S2 

0  - 

A°(tfi)  = 

Si 

0 

0 

-1 

Cl 

0 

0 

0 

A\{d2)  = 

S2 

0 

0 

1 

-C2 

0 

0 

d2 

.  0 

0 

0 

1. 

.  0 

0 

0 

1  . 

-^■3(^3) 


-100  0- 
0  10  0 
0  0  1  d3 
.0  0  0  1  . 


10  Notice  that  it  is  not  possible  to  solve  (2.64)  for  d2' 
strained  by  the  condition  $2'  +  $3'  =  n. 


and  d3>  since  they  are  con- 
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Fig.  2.22.  Spherical  arm 


Computation  of  the  direct  kinematics  function  as  in  (2.50)  yields 


'cic2 

-Sl 

cis2 

C1S2CZ3  -  Sid2 

A0  A1 A2  — 

sic2 

Cl 

sis2 

Sl  S2C?3  +  Cid2 

-82 

0 

C2 

C2d3 

.  0 

0 

0 

1 

(2.65) 


where  q  =  [$i  $2  Notice  that  the  third  joint  does  not  obviously 

influence  the  rotation  matrix.  Further,  the  orientation  of  the  unit  vector  ylj 
is  uniquely  determined  by  the  first  joint,  since  the  revolute  axis  of  the  second 
joint  Z\  is  parallel  to  axis  y3.  Different  from  the  previous  structures,  in  this 
case  Frame  3  can  represent  an  end-effector  frame  of  unit  vectors  (ne,se,ae), 
i-e.,  T\  =  I4. 


2.9.4  Anthropomorphic  Arm 

Consider  the  anthropomorphic  arm  in  Fig.  2.23.  Notice  how  this  arm  corre¬ 
sponds  to  a  two-link  planar  arm  with  an  additional  rotation  about  an  axis 
of  the  plane.  In  this  respect,  the  parallelogram  arm  could  be  used  in  lieu  of 
the  two-link  planar  arm,  as  found  in  some  industrial  robots  with  an  anthro¬ 
pomorphic  structure. 

The  link  frames  have  been  illustrated  in  the  figure.  As  for  the  previous 
structure,  the  origin  of  Frame  0  was  chosen  at  the  intersection  of  zq  with  Z\ 
(d\  =  0);  further,  Z\  and  2 2  are  parallel  and  the  choice  of  axes  X\  and  X2 
was  made  as  for  the  two-link  planar  arm.  The  DH  parameters  are  specified  in 
Table  2.4. 
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Table  2.4.  DH  parameters  for  the  anthropomorphic  arm 


Link 

di 

Oli 

di 

di 

1 

0 

7t/2 

0 

di 

2 

02 

0 

0 

d2 

3 

03 

0 

0 

d3 

The  homogeneous  transformation  matrices  defined  in  (2.52)  are  for  the 
single  joints: 


‘Ci  0  Si  O' 

si  0  —Ci  0 
0  10  0 
.  0  0  0  1. 


L(^) 


'Ci 

Si 

0  ciiCi 

Si 

Ci 

0  CLiSi 

0 

0 

1 

0 

.  0 

0 

0 

1 

i  =  2,3. 


Computation  of  the  direct  kinematics  function  as  in  (2.50)  yields 


T°(q)  =  A\A^A\ 


’ClC23  ~ ClS23  S1  Ci(tt2C2  T  U3C23) 

S1C23  — S1S23  —Cl  Si(tt2C2  T  C.3C23) 

S23  C23  0  02^2  +  CI3S23 

.  0  0  0  1 


(2.66) 


where  q  =  [i?i  $2  $3 }T ■  Since  Z3  is  aligned  with  Z2,  Frame  3  does  not  coin¬ 

cide  with  a  possible  end-effector  frame  as  in  Fig.  2.13,  and  a  proper  constant 
transformation  would  be  needed. 
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2.9.5  Spherical  Wrist 

Consider  a  particular  type  of  structure  consisting  just  of  the  wrist  of  Fig.  2.24. 
Joint  variables  were  numbered  progressively  starting  from  4,  since  such  a 
wrist  is  typically  thought  of  as  mounted  on  a  three-DOF  arm  of  a  six-DOF 
manipulator.  It  is  worth  noticing  that  the  wrist  is  spherical  since  all  revolute 
axes  intersect  at  a  single  point.  Once  Z3,  Z4,  z 5  have  been  established,  and  X3 
has  been  chosen,  there  is  an  indeterminacy  on  the  directions  of  X4  and  X5. 
With  reference  to  the  frames  indicated  in  Fig.  2.24,  the  DH  parameters  are 
specified  in  Table  2.5. 


Table  2.5.  DH  parameters  for  the  spherical  wrist 


Link 

ai 

ai 

di 

A 

4 

0 

— 7r/2 

0 

A 

5 

0 

7t/2 

0 

A 

6 

0 

0 

de 

$6 

The  homogeneous  transformation  matrices  defined  in  (2.52)  are  for  the 
single  joints: 


"c4 

0 

-S4 

0- 

■c5 

0 

S5 

0- 

A\{#  4)  = 

s4 

0 

0 

-1 

c4 

0 

0 

0 

a£o?5)  = 

S5 

0 

0 

1 

-c5 

0 

0 

0 

.  0 

0 

0 

1. 

.  0 

0 

0 

1. 

Cq  Sq  0  0 

s6  c6  0  0 

0  0  1  d6 

. 0  0  01. 


76 


2  Kinematics 


Fig.  2.25.  Stanford  manipulator 


Computation  of  the  direct  kinematics  function  as  in  (2.50)  yields 

C4C5C5  S4S6  C4C5S6  S4CQ  C4.S3  C4.S§(Iq 
S4C3CQ  T  C4S6  — S4C5S6  +  C4C6  S4S5  S4S5CIQ 
S5C6  S5S6  C5  C§(Iq 

0  0  0  1  . 

(2.67) 

where  q  =  [i?4  i}5  ,&e]T ■  Notice  that,  as  a  consequence  of  the  choice  made 

for  the  coordinate  frames,  the  block  matrix  Rq  that  can  be  extracted  from  T j? 
coincides  with  the  rotation  matrix  of  Euler  angles  (2.18)  previously  derived, 
that  is,  $4,  $5,  $6  constitute  the  set  of  ZYZ  angles  with  respect  to  the  reference 
frame  O3-X31/3Z3.  Moreover,  the  unit  vectors  of  Frame  6  coincide  with  the  unit 
vectors  of  a  possible  end-effector  frame  according  to  Fig.  2.13. 


2.9.6  Stanford  Manipulator 

The  so-called  Stanford  manipulator  is  composed  of  a  spherical  arm  and  a 
spherical  wrist  (Fig.  2.25).  Since  Frame  3  of  the  spherical  arm  coincides  with 
Frame  3  of  the  spherical  wrist,  the  direct  kinematics  function  can  be  obtained 
via  simple  composition  of  the  transformation  matrices  (2.65),  (2.67)  of  the 
previous  examples,  i.e., 
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Carrying  out  the  products  yields 


C\S2d^  —  S\d2  +  (ci  (C2C4S5  +  S2C5)  —  SiS4Ss)(i6 
SlS2C?3  +  Cid2  +  (si(c2c4s5  +  S2C5)  +  CiS4S5)d6 
C2d3  +  (-S2C4S5  +  c2c5)de 


for  the  end-effector  position,  and 


n 


0 

6 


S 


0 

6 


a 


0 

6 


^1 (02 (C4C5C6  —  S4Sg)  —  S2S5C6)  —  Sl(s4C5C6  +  C4S6) 

Si  (c2(C4C3Cq  —  S4S6)  ~  S2S5C6)  +  Ci(s4C5C6  +  C4S6) 

—  S2(C4C5C6  —  S4S6)  —  C2S5C6 
Cl  (  — C2(c4C5S6  +  S4Cq)  +  S2S5S6)  —  Si(  —  S4C5S6  +  C4Cg) 
Si  (  — C2(c4C5S6  +  S4C6)  +  S2S5S6)  +  Ci(  —  S4C5S6  +  C4C6) 
S2(c4C5S6  +  S4C6)  +  C2S5S6 

Cl  (C2C4S5  +  S2C5)  —  S1S4S5 
Si  (C2C4S5  +  S2C5)  +  C1S4S5 
— S2C4S5  +  C2C5 


(2.68) 


(2.69) 


for  the  end-effector  orientation. 

A  comparison  of  the  vector  in  (2.68)  with  the  vector  pg  in  (2.65)  relative 
to  the  sole  spherical  arm  reveals  the  presence  of  additional  contributions  due 
to  the  choice  of  the  origin  of  the  end-effector  frame  at  a  distance  de  from 
the  origin  of  Frame  3  along  the  direction  of  a g.  In  other  words,  if  it  were 
de  =  0,  the  position  vector  would  be  the  same.  This  feature  is  of  fundamental 
importance  for  the  solution  of  the  inverse  kinematics  for  this  manipulator,  as 
will  be  seen  later. 


2.9.7  Anthropomorphic  Arm  with  Spherical  Wrist 

A  comparison  between  Fig.  2.23  and  Fig.  2.24  reveals  that  the  direct  kinemat¬ 
ics  function  cannot  be  obtained  by  multiplying  the  transformation  matrices 
Tg  and  Tg,  since  Frame  3  of  the  anthropomorphic  arm  cannot  coincide  with 
Frame  3  of  the  spherical  wrist. 

Direct  kinematics  of  the  entire  structure  can  be  obtained  in  two  ways. 
One  consists  of  interposing  a  constant  transformation  matrix  between  Tg  and 
Tg  which  allows  the  alignment  of  the  two  frames.  The  other  refers  to  the 
Denavit-Hartenberg  operating  procedure  with  the  frame  assignment  for  the 
entire  structure  illustrated  in  Fig.  2.26.  The  DH  parameters  are  specified  in 
Table  2.6. 

Since  Rows  3  and  4  differ  from  the  corresponding  rows  of  the  tables  for 
the  two  single  structures,  the  relative  homogeneous  transformation  matrices 
Ag  and  A\  have  to  be  modified  into 


"C3 

0 

S3 

0- 

'C4 

0 

-s4 

0  ' 

^3(^3)  = 

S3 

0 

0 

1 

-C3 

0 

0 

0 

A34(d4)  = 

S4 

0 

0 

-1 

c4 

0 

0 

d4 

.  0 

0 

0 

1. 

.  0 

0 

0 

1  . 
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Fig.  2.26.  Anthropomorphic  arm  with  spherical  wrist 


Table  2.6.  DH  parameters  for  the  anthropomorphic  arm  with  spherical  wrist 


Link 

ai 

O-i 

di 

0; 

1 

0 

tt/2 

0 

di 

2 

ai 

0 

0 

#2 

3 

0 

tt/2 

0 

$3 

4 

0 

— tt/2 

di 

04 

5 

0 

tt/2 

0 

05 

6 

0 

0 

d@ 

06 

while  the  other  transformation  matrices  remain  the  same.  Computation  of  the 
direct  kinematics  function  leads  to  expressing  the  position  and  orientation  of 
the  end-effector  frame  as: 


Pe  = 


CI2C1C2  +  d4ClS23  +  de(ci(C23C4S5  +  S23C5)  +  S1S4S5) 
CI2S1C2  +  d^SiS23  +  d6(si(c23C4S5  +  S23C5)  —  C1S4S5) 
(I2S2  —  d±C23  +  de(s23c4S5  —  C23C5) 


(2.70) 


and 


n 


0 

6 


S 


0 

6 


a 


0 

6 


C1  (C23(C4C5C6  —  S4S6)  —  S23S5C6)  +  Si(S4C5C6  +  C4S6) 

S1  (C23(C4C5C6  —  S4S6)  —  S23S5C6)  Ci(S4C5C6  +  C4S6) 

S23(C4C5C6  —  S4S6)  +  C23S5C6 

Cl  (  — C23(C4C5S6  +  S4C6)  +  S23S5S6)  +  Si(  —  S4C5S6  +  C4C6) 
Si  (~  C23(C4C5S6  +  S4C6)  +  S23S5S6)  —  Ci(  — S4C5S6  +  C4C6) 
^•823(0405,86  +  S4C6)  —  C23S5S6 
Cl  (C23C4S5  +  S23C5)  +  S1S4S5 
Si  (C23C485  +  S23C5)  —  C1S4S5 
S23C4S5  —  C23C5 


(2.71) 
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By  setting  d$  =  0,  the  position  of  the  wrist  axes  intersection  is  obtained.  In 
that  case,  the  vector  p°  in  (2.70)  corresponds  to  the  vector  for  the  sole 
anthropomorphic  arm  in  (2.66),  because  d4  gives  the  length  of  the  forearm 
(CI3)  and  axis  X3  in  Fig.  2.26  is  rotated  by  7r/2  with  respect  to  axis  X3  in 
Fig.  2.23. 


2.9.8  DLR  Manipulator 

Consider  the  DLR  manipulator,  whose  development  is  at  the  basis  of  the  real¬ 
ization  of  the  robot  in  Fig.  1.30;  it  is  characterized  by  seven  DOFs  and  as  such 
it  is  inherently  redundant.  This  manipulator  has  two  possible  configurations 
for  the  outer  three  joints  (wrist).  With  reference  to  a  spherical  wrist  similar  to 
that  introduced  in  Sect.  2.9.5,  the  resulting  kinematic  structure  is  illustrated 
in  Fig.  2.27,  where  the  frames  attached  to  the  links  are  evidenced. 

As  in  the  case  of  the  spherical  arm,  notice  that  the  origin  of  Frame  0  has 
been  chosen  so  as  to  zero  d\.  The  DH  parameters  are  specified  in  Table  2.7. 


Table  2.7.  DH  parameters  for  the  DLR  manipulator 


Link 

di 

a» 

di 

di 

1 

0 

7t/2 

0 

di 

2 

0 

7t/2 

0 

d2 

3 

0 

tt/2 

d3 

d3 

4 

0 

tt/2 

0 

d4 

5 

0 

tt/2 

d$ 

d  5 

6 

0 

7t/2 

0 

d  6 

7 

0 

0 

dj 

d  7 
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The  generic  homogeneous  transformation  matrix  defined  in  (2.52)  is  (a,  = 
tt/2) 

'  Ci  0  Si  0 

Si  0  —  Ci  0 

0  1  0  di 

LOO  0  1  J 


A,  = 


i  =  1, . . . ,  6 


(2.72) 


while,  since  07  =  0,  it  is 


~c7 

-57 

0 

0 

57 

07 

0 

0 

0 

0 

1 

d7 

.  0 

0 

0 

1 

(2.73) 


The  direct  kinematics  function,  computed  as  in  (2.50),  leads  to  the  following 
expressions  for  the  end-effector  frame 


r>°  - 

P  7  ~ 


d3xds  +  d5xd5  +  d7xdy 
d3Vd3  +  d5yd5  +  d7ydj 
d3zd3  +  d$Zd5  +  d7Zd7 


(2.74) 


with 


Xd%  §2 

Xd§  ^1(^2^354  ^2^4)  “1“  S1S3S4 

Xd7  =  Ci(c2fci  +  S2k2)  +  S1/C3 
Vd3  =  S1S2 

Dd3  =  Sl(c2C3S4  —  S2C4)  —  C1S3S4 
Vd7  =  Si(c2fci  +  s2k2)  -  Cik3 

Zd3  =  ~c2 

Zd5  =  C2C4  +  S2C3S4 

Zd7  =  ^2(^3(040553  —  S4C6)  +  S3S5S6)  —  c2k2, 

where 

k\  —  03(040556  54C6)  -t-  535556 

k2  =  S4C5S6  +  C4C6 

k3  =  53(040556  —  S4C6)  —  C3S5S6. 


Furthermore,  the  end-effector  frame  orientation  can  be  derived  as 


n 


0 

7 


(( Xac5  +  xcs5)c6  +  xbs6)c7  +  (xas5  -  XcC5)s7 
(( yac5  +  ycS5)ce  +  2/&s6)c7  +  (yas5  -  ycc5)s7 
(zac6  +  zcs6)c7  +  zbs7 
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s 


o 

7 


a 


o 

7 


((xaC5  T"  XqS^Cq  T"  XjjSq^Sj  (xaS 5  *£<2^5)  ^7 

~((yaC5  +  ycS5)ce  +  Vbse)s7  +  (yas5  ~  ycCb)c7 
-(zace  +  zcs6)s7  +  zbc7 

{xaC5  +  XcSb)sg  —  XbCQ 
(yaC5+ycS5)S6-ybC6  , 


where 


(2.75) 


xa  =  (C1C2C3  +  SiS3)C4  +  C1S2S4 

Xb  =  (C1C2C3  +  <5453)54  —  C1S2C4 

Xc  =  C1C2S3  —  S1C3 

2/o  =  (S1C2C3  —  CiS3)C4  +  S1S2S4 

2/6  =  (S1C2C3  —  CiS3)S4  —  S1S2C4 

Vc  =  S1C2S3  +  C1C3 

Za  =  (S2C3C4  —  C2S4)C5  +  S2S3S5 

Zb  —  (S2C3C4  —  C2S4)S5  —  S2S3C5 

Zc  =  S2C3S4  +  C2C4. 

(2.76) 


As  in  the  case  of  the  anthropomorphic  arm  with  spherical  wrist,  it  occurs 
that  Frame  4  cannot  coincide  with  the  base  frame  of  the  wrist. 

Finally,  consider  the  possibility  to  mount  a  different  type  of  spherical  wrist, 
where  Joint  7  is  so  that  a7  =  7r/2.  In  such  a  case,  the  computation  of  the 
direct  kinematics  function  changes,  since  the  seventh  row  of  the  kinematic 
parameters  table  changes.  In  particular,  notice  that,  since  d7  =  0,  a7  ^  0, 
then 


'C7 

0 

s7 

a7c7 

s7 

0 

-or 

a7s7 

0 

0 

1 

0 

.  0 

0 

0 

1 

(2.77) 


It  follows,  however,  that  Frame  7  does  not  coincide  with  the  end-effector 
frame,  as  already  discussed  for  the  three-link  planar  arm,  since  the  approach 
unit  vector  a!)  is  aligned  with  x7. 


2.9.9  Humanoid  Manipulator 

The  term  humanoid  refers  to  a  robot  showing  a  kinematic  structure  similar  to 
that  of  the  human  body.  It  is  commonly  thought  that  the  most  relevant  fea¬ 
ture  of  humanoid  robots  is  biped  locomotion.  However,  in  detail,  a  humanoid 
manipulator  refers  to  an  articulated  structure  with  a  kinematics  analogous  to 


82 


2  Kinematics 


that  of  the  human  body  upper  part:  torso,  arms,  end-effectors  similar  to  hu¬ 
man  hands  and  a  ‘head’  which,  eventually,  includes  an  artificial  vision  system 
—  see  Chap.  10. 

For  the  humanoid  manipulator  in  Fig.  1.33,  it  is  worth  noticing  the  pres¬ 
ence  of  two  end-effectors  (where  the  ‘hands’  are  mounted),  while  the  arms 
consist  of  two  DLR  manipulators,  introduced  in  the  previous  section,  each 
with  seven  DOFs.  In  particular,  consider  the  configuration  where  the  last 
joint  is  so  that  ay  =  7r/2. 

To  simplify,  the  kinematic  structure  allowing  the  articulation  of  the  robot’s 
head  in  Fig.  1.33.  The  torso  can  be  modelled  as  an  anthropomorphic  arm 
(three  DOFs),  for  a  total  of  seventeen  DOFs. 

Further,  a  connecting  device  exists  between  the  end-effector  of  the  anthro¬ 
pomorphic  torso  and  the  base  frames  of  the  two  manipulators.  Such  device 
permits  keeping  the  ‘chest’  of  the  humanoid  manipulator  always  orthogonal  to 
the  ground.  With  reference  to  Fig.  2.28,  this  device  is  represented  by  a  further 
joint,  located  at  the  end  of  the  torso.  Hence,  the  corresponding  parameter  $4 
does  not  constitute  a  DOF,  yet  it  varies  so  as  to  compensate  Joints  2  and  3 
rotations  of  the  anthropomorphic  torso. 

To  compute  the  direct  kinematics  function,  it  is  possible  to  resort  to  a  DH 
parameters  table  for  each  of  the  two  tree  kinematic  structures,  which  can  be 
identified  from  the  base  of  the  manipulator  to  each  of  the  two  end-effectors. 
Similarly  to  the  case  of  mounting  a  spherical  wrist  onto  an  anthropomorphic 
arm,  this  implies  the  change  of  some  rows  of  the  transformation  matrices  of 
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those  manipulators,  described  in  the  previous  sections,  constituting  the  torso 
and  the  arms. 

Alternatively,  it  is  possible  to  consider  intermediate  transformation  matri¬ 
ces  between  the  relevant  structures.  In  detail,  as  illustrated  in  Fig.  2.28,  if  t, 
denotes  the  frame  attached  to  the  torso,  r  and  l  the  base  frames,  respectively, 
of  the  right  arm  and  the  left  arm,  and  rh  and  Ih  the  frames  attached  to  the 
two  hands  (end-effectors),  it  is  possible  to  compute  for  the  right  arm  and  the 
left  arm,  respectively: 


T°rh  =  T°  It  TlTrrh  (2.78) 

Taih  =  T°  it  T\T\h  (2.79) 

where  the  matrix  it  describes  the  transformation  imposed  by  the  motion  of 
Joint  4  (dashed  line  in  Fig.  2.28),  located  at  the  end-effector  of  the  torso. 
Frame  4  coincides  with  Frame  t  in  Fig.  2.27.  In  view  of  the  property  of  pa¬ 
rameter  tb,  it  is  $4  =  —$2  —  $3,  and  thus 

C23  s23  0  0 " 

S23  C23  0  0 

0  0  10' 

0  0  0  1. 

The  matrix  is  given  by  (2.66),  whereas  the  matrices  T*  and  T\  relating 
the  torso  end-effector  frame  to  the  base  frames  of  the  two  manipulators  have 
constant  values.  With  reference  to  Fig.  2.28,  the  elements  of  these  matrices 
depend  on  the  angle  (5  and  on  the  distances  between  the  origin  of  Frame  t 
and  the  origins  of  Frames  r  and  l.  Finally,  the  expressions  of  the  matrices  Trrh 
and  Tllh  must  be  computed  by  considering  the  change  in  the  seventh  row  of 
the  DH  parameters  table  of  the  DLR  manipulator,  so  as  to  account  for  the 
different  kinematic  structure  of  the  wrist  (see  Problem  2.14). 


2.10  Joint  Space  and  Operational  Space 

As  described  in  the  previous  sections,  the  direct  kinematics  equation  of  a 
manipulator  allows  the  position  and  orientation  of  the  end-effector  frame  to 
be  expressed  as  a  function  of  the  joint  variables  with  respect  to  the  base  frame. 

If  a  task  is  to  be  specified  for  the  end-effector,  it  is  necessary  to  assign  the 
end-effector  position  and  orientation,  eventually  as  a  function  of  time  (tra¬ 
jectory).  This  is  quite  easy  for  the  position.  On  the  other  hand,  specifying 
the  orientation  through  the  unit  vector  triplet  (ne,  se,  ae)n  is  quite  difficult, 
since  their  nine  components  must  be  guaranteed  to  satisfy  the  orthonormal¬ 
ity  constraints  imposed  by  (2.4)  at  each  time  instant.  This  problem  will  be 
resumed  in  Chap.  4. 


11 


To  simplify,  the  indication  of  the  reference  frame  in  the  superscript  is  omitted. 
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The  problem  of  describing  end-effector  orientation  admits  a  natural  so¬ 
lution  if  one  of  the  above  minimal  representations  is  adopted.  In  this  case, 
indeed,  a  motion  trajectory  can  be  assigned  to  the  set  of  angles  chosen  to 
represent  orientation. 

Therefore,  the  position  can  be  given  by  a  minimal  number  of  coordinates 
with  regard  to  the  geometry  of  the  structure,  and  the  orientation  can  be 
specified  in  terms  of  a  minimal  representation  (Euler  angles)  describing  the 
rotation  of  the  end-effector  frame  with  respect  to  the  base  frame.  In  this  way, 
it  is  possible  to  describe  the  end-effector  pose  by  means  of  the  (m  x  1)  vector, 
with  m  <  n, 


x 


e 


Pe 

4>e 


(2.80) 


where  pe  describes  the  end-effector  position  and  <pe  its  orientation. 

This  representation  of  position  and  orientation  allows  the  description  of  an 
end-effector  task  in  terms  of  a  number  of  inherently  independent  parameters. 
The  vector  xe  is  defined  in  the  space  in  which  the  manipulator  task  is  specified; 
hence,  this  space  is  typically  called  operational  space.  On  the  other  hand,  the 
joint  space  (configuration  space)  denotes  the  space  in  which  the  (n  x  1)  vector 
of  joint  variables 

M 


q  = 


(2.81) 


Qn 


is  defined;  it  is  qi  =  d,  for  a  revolute  joint  and  qi  =  di  for  a  prismatic 
joint.  Accounting  for  the  dependence  of  position  and  orientation  from  the 
joint  variables,  the  direct  kinematics  equation  can  be  written  in  a  form  other 
than  (2.50),  i.e. , 

xe  =  k(q).  (2.82) 


The  (to  x  1)  vector  function  fc(-)  —  nonlinear  in  general  —  allows  computa¬ 
tion  of  the  operational  space  variables  from  the  knowledge  of  the  joint  space 
variables. 

It  is  worth  noticing  that  the  dependence  of  the  orientation  components 
of  the  function  k(q)  in  (2.82)  on  the  joint  variables  is  not  easy  to  express 
except  for  simple  cases.  In  fact,  in  the  most  general  case  of  a  six-dimensional 
operational  space  (to  =  6),  the  computation  of  the  three  components  of  the 
function  <fie(q)  cannot  be  performed  in  closed  form  but  goes  through  the 
computation  of  the  elements  of  the  rotation  matrix,  i.e.,  ne(q ),  se(q),  ae(q). 
The  equations  that  allow  the  determination  of  the  Euler  angles  from  the  triplet 
of  unit  vectors  ne,  se,  ae  were  given  in  Sect.  2.4. 
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Example  2.5 

Consider  again  the  three-link  planar  arm  in  Fig.  2.20.  The  geometry  of  the  structure 
suggests  that  the  end-effector  position  is  determined  by  the  two  coordinates  px  and 
py,  while  its  orientation  is  determined  by  the  angle  <j>  formed  by  the  end-effector  with 
the  axis  xq.  Expressing  these  operational  variables  as  a  function  of  the  joint  variables, 
the  two  position  coordinates  are  given  by  the  first  two  elements  of  the  fourth  column 
of  the  homogeneous  transformation  matrix  (2.63),  while  the  orientation  angle  is 
simply  given  by  the  sum  of  joint  variables.  In  sum,  the  direct  kinematics  equation 
can  be  written  in  the  form 


~Px~ 

aici  +  a2Ci2  +  U3C123 

Xe  = 

Py 

=  k{q)  = 

aisi  +  Q12S12  +  U3S123 

.  <t>  . 

$1  +  1?2  +  &3 

This  expression  shows  that  three  joint  space  variables  allow  specification  of  at  most 
three  independent  operational  space  variables.  On  the  other  hand,  if  orientation  is 
of  no  concern,  it  is  xe  =  [px  py]T  and  there  is  kinematic  redundancy  of  DOFs 
with  respect  to  a  pure  positioning  end-effector  task;  this  concept  will  be  dealt  with 
in  detail  afterwards. 


2.10.1  Workspace 

With  reference  to  the  operational  space,  an  index  of  robot  performance  is 
the  so-called  workspace ;  this  is  the  region  described  by  the  origin  of  the  end- 
effector  frame  when  all  the  manipulator  joints  execute  all  possible  motions.  It 
is  often  customary  to  distinguish  between  reachable  workspace  and  dexterous 
workspace.  The  latter  is  the  region  that  the  origin  of  the  end-effector  frame 
can  describe  while  attaining  different  orientations,  while  the  former  is  the 
region  that  the  origin  of  the  end-effector  frame  can  reach  with  at  least  one 
orientation.  Obviously,  the  dexterous  workspace  is  a  subspace  of  the  reachable 
workspace.  A  manipulator  with  less  than  six  DOFs  cannot  take  any  arbitrary 
position  and  orientation  in  space. 

The  workspace  is  characterized  by  the  manipulator  geometry  and  the  me¬ 
chanical  joint  limits.  For  an  n-DOF  manipulator,  the  reachable  workspace  is 
the  geometric  locus  of  the  points  that  can  be  achieved  by  considering  the 
direct  kinematics  equation  for  the  sole  position  part,  i.e., 

Pe  =  Pe(q)  Qim  <  Qi  <  QiM 

where  qim  ( qiM )  denotes  the  minimum  (maximum)  limit  at  Joint  i.  This  vol¬ 
ume  is  finite,  closed,  connected  pe(t j)  is  a  continuous  function  —  and  thus 
is  defined  by  its  bordering  surface.  Since  the  joints  are  revolute  or  prismatic, 
it  is  easy  to  recognize  that  this  surface  is  constituted  by  surface  elements  of 
planar,  spherical,  toroidal  and  cylindrical  type.  The  manipulator  workspace 


2  Kinematics 


A 

% 


6 

c 

qut 

qi 

'w 

a 

q2=  0 

d 

Fig.  2.29.  Region  of  admissible  configurations  for  a  two-link  arm 


(without  end-effector)  is  reported  in  the  data  sheet  given  by  the  robot  manu¬ 
facturer  in  terms  of  a  top  view  and  a  side  view.  It  represents  a  basic  element 
to  evaluate  robot  performance  for  a  desired  application. 


Example  2.6 

Consider  the  simple  two-link  planar  arm.  If  the  mechanical  joint  limits  are  known, 
the  arm  can  attain  all  the  joint  space  configurations  corresponding  to  the  points  in 
the  rectangle  in  Fig.  2.29. 

The  reachable  workspace  can  be  derived  via  a  graphical  construction  of  the 
image  of  the  rectangle  perimeter  in  the  plane  of  the  arm.  To  this  purpose,  it  is 
worth  considering  the  images  of  the  segments  ab,  be,  cd,  da,  ae,  ef,  fd.  Along  the 
segments  ab,  be,  cd,  ae,  ef,  fd  a  loss  of  mobility  occurs  due  to  a  joint  limit;  a 
loss  of  mobility  occurs  also  along  the  segment  ad  because  the  arm  and  forearm  are 
aligned.12  Further,  a  change  of  the  arm  posture  occurs  at  points  a  and  d:  for  qi  >  0 
the  elbow-down  posture  is  obtained,  while  for  q%  <  0  the  arm  is  in  the  elbow-up 
posture. 

In  the  plane  of  the  arm,  start  drawing  the  arm  in  configuration  A  corresponding 
to  q\m  and  q2  =  0  (a);  then,  the  segment  ab  describing  motion  from  52  =  0  to 
q-2M  generates  the  arc  AB;  the  subsequent  arcs  BC ,  CD,  DA,  AE,  EF,  FD  are 
generated  in  a  similar  way  (Fig.  2.30).  The  external  contour  of  the  area  CDAEFHC 
delimits  the  requested  workspace.  Further,  the  area  BCDAB  is  relative  to  elbow- 
down  postures  while  the  area  DAEFD  is  relative  to  elbow-up  postures;  hence,  the 
points  in  the  area  BADFbB  are  reachable  by  the  end-effector  with  both  postures. 


12 


In  the  following  chapter,  it  will  be  seen  that  this  configuration  characterizes  a 
kinematic  singularity  of  the  arm. 
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Fig.  2.30.  Workspace  of  a  two-link  planar  arm 


In  a  real  manipulator,  for  a  given  set  of  joint  variables,  the  actual  val¬ 
ues  of  the  operational  space  variables  deviate  from  those  computed  via  direct 
kinematics.  The  direct  kinematics  equation  has  indeed  a  dependence  from  the 
DH  parameters  which  is  not  explicit  in  (2.82).  If  the  mechanical  dimensions 
of  the  structure  differ  from  the  corresponding  parameter  of  the  table  because 
of  mechanical  tolerances,  a  deviation  arises  between  the  position  reached  in 
the  assigned  posture  and  the  position  computed  via  direct  kinematics.  Such  a 
deviation  is  defined  accuracy ;  this  parameter  attains  typical  values  below  one 
millimeter  and  depends  on  the  structure  as  well  as  on  manipulator  dimen¬ 
sions.  Accuracy  varies  with  the  end-effector  position  in  the  workspace  and  it 
is  a  relevant  parameter  when  robot  programming  oriented  environments  are 
adopted,  as  will  be  seen  in  Chap.  6. 

Another  parameter  that  is  usually  listed  in  the  performance  data  sheet  of 
an  industrial  robot  is  repeatability  which  gives  a  measure  of  the  manipulator’s 
ability  to  return  to  a  previously  reached  position;  this  parameter  is  relevant 
for  programming  an  industrial  robot  by  the  teaching-by-showing  technique 
which  will  be  presented  in  Chap.  6.  Repeatability  depends  not  only  on  the 
characteristics  of  the  mechanical  structure  but  also  on  the  transducers  and 
controller;  it  is  expressed  in  metric  units  and  is  typically  smaller  than  accuracy. 
For  instance,  for  a  manipulator  with  a  maximum  reach  of  1.5  m,  accuracy 
varies  from  0.2  to  1mm  in  the  workspace,  while  repeatability  varies  from  0.02 
to  0.2  mm. 

2.10.2  Kinematic  Redundancy 

A  manipulator  is  termed  kinematically  redundant  when  it  has  a  number  of 
DOFs  which  is  greater  than  the  number  of  variables  that  are  necessary  to 
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describe  a  given  task.  With  reference  to  the  above-defined  spaces,  a  manipu¬ 
lator  is  intrinsically  redundant  when  the  dimension  of  the  operational  space  is 
smaller  than  the  dimension  of  the  joint  space  (m  <  n).  Redundancy  is,  any¬ 
how,  a  concept  relative  to  the  task  assigned  to  the  manipulator;  a  manipulator 
can  be  redundant  with  respect  to  a  task  and  nonredundant  with  respect  to 
another.  Even  in  the  case  of  m  =  n,  a  manipulator  can  be  functionally  redun¬ 
dant  when  only  a  number  of  r  components  of  operational  space  are  of  concern 
for  the  specific  task,  with  r  <  m. 

Consider  again  the  three-DOF  planar  arm  of  Sect.  2.9.1.  If  only  the  end- 
effector  position  (in  the  plane)  is  specified,  that  structure  presents  a  functional 
redundancy  (n  =  m  =  3,  r  =  2);  this  is  lost  when  also  the  end-effector 
orientation  in  the  plane  is  specified  (n  =  m  =  r  =  3).  On  the  other  hand,  a 
four-DOF  planar  arm  is  intrinsically  redundant  (n  =  4,  m  =  3). 

Yet,  take  the  typical  industrial  robot  with  six  DOFs;  such  manipulator 
is  not  intrinsically  redundant  (n  =  to  =  6),  but  it  can  become  functionally 
redundant  with  regard  to  the  task  to  execute.  Thus,  for  instance,  in  a  laser¬ 
cutting  task  a  functional  redundancy  will  occur  since  the  end-effector  rotation 
about  the  approach  direction  is  irrelevant  to  completion  of  the  task  (r  =  5). 

At  this  point,  a  question  should  arise  spontaneously:  Why  to  intentionally 
utilize  a  redundant  manipulator?  The  answer  is  to  recognize  that  redundancy 
can  provide  the  manipulator  with  dexterity  and  versatility  in  its  motion.  The 
typical  example  is  constituted  by  the  human  arm  that  has  seven  DOFs:  three 
in  the  shoulder,  one  in  the  elbow  and  three  in  the  wrist,  without  considering 
the  DOFs  in  the  fingers.  This  manipulator  is  intrinsically  redundant;  in  fact, 
if  the  base  and  the  hand  position  and  orientation  are  both  fixed  requiring 
six  DOFs  —  the  elbow  can  be  moved,  thanks  to  the  additional  available  DOF. 
Then,  for  instance,  it  is  possible  to  avoid  obstacles  in  the  workspace.  Further, 
if  a  joint  of  a  redundant  manipulator  reaches  its  mechanical  limit,  there  might 
be  other  joints  that  allow  execution  of  the  prescribed  end-effector  motion. 

A  formal  treatment  of  redundancy  will  be  presented  in  the  following  chap¬ 
ter. 


2.11  Kinematic  Calibration 

The  Denavit-Hartenberg  parameters  for  direct  kinematics  need  to  be  com¬ 
puted  as  precisely  as  possible  in  order  to  improve  manipulator  accuracy.  Kine¬ 
matic  calibration  techniques  are  devoted  to  finding  accurate  estimates  of  DH 
parameters  from  a  series  of  measurements  on  the  manipulator’s  end-effector 
pose.  Hence,  they  do  not  allow  direct  measurement  of  the  geometric  parame¬ 
ters  of  the  structure. 

Consider  the  direct  kinematics  equation  in  (2.82)  which  can  be  rewritten 
by  emphasizing  the  dependence  of  the  operational  space  variables  on  the  fixed 
DH  parameters,  besides  the  joint  variables.  Let  a  =  [a  i  ...  an  ]T ,  a  = 
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[a\  ...  an]T ,  d  =  [d\  ...  dn]T ,  and  ft  =  [6\  ...  dn]T  denote  the 

vectors  of  DH  parameters  for  the  whole  structure;  then  (2.82)  becomes 

xe  =  k(a,  a,  d,  ft).  (2.84) 


The  manipulator’s  end-effector  pose  should  be  measured  with  high  precision 
for  the  effectiveness  of  the  kinematic  calibration  procedure.  To  this  purpose 
a  mechanical  apparatus  can  be  used  that  allows  the  end-effector  to  be  con¬ 
strained  at  given  poses  with  a  priori  known  precision.  Alternatively,  direct 
measurement  systems  of  object  position  and  orientation  in  the  Cartesian  space 
can  be  used  which  employ  triangulation  techniques. 

Let  xm  be  the  measured  pose  and  xn  the  nominal  pose  that  can  be  com¬ 
puted  via  (2.84)  with  the  nominal  values  of  the  parameters  a,  a ,  d,  ft.  The 
nominal  values  of  the  fixed  parameters  are  set  equal  to  the  design  data  of  the 
mechanical  structure,  whereas  the  nominal  values  of  the  joint  variables  are  set 
equal  to  the  data  provided  by  the  position  transducers  at  the  given  manipula¬ 
tor  posture.  The  deviation  Ax  =  xm  —  xn  gives  a  measure  of  accuracy  at  the 
given  posture.  On  the  assumption  of  small  deviations,  at  first  approximation, 
it  is  possible  to  derive  the  following  relation  from  (2.84): 


Ax 


dk  9k  9k  9k 
—  Aa  +  -da  +  —Ad  +  —Aft 
da  da  ad  aft 


(2.85) 


where  Aa ,  Aa,  Ad,  Aft  denote  the  deviations  between  the  values  of  the 
parameters  of  the  real  structure  and  the  nominal  ones.  Moreover,  dk/da, 
dk/da ,  dk/dd,  dk/dft  denote  the  (m  x  n)  matrices  whose  elements  are  the 
partial  derivatives  of  the  components  of  the  direct  kinematics  function  with 
respect  to  the  single  parameters.13 

Group  the  parameters  in  the  (4n  x  1)  vector  C  =  [ aT  aT  dT  ftT]T . 
Let  AC  =  Cm — Cn  denote  the  parameter  variations  with  respect  to  the  nominal 
values,  and  ^  =  [dk/da  dk/ da  dk /dd  dk /dft ]  the  (m  x  4n)  kinematic 
calibration  matrix  computed  for  the  nominal  values  of  the  parameters  Cn- 
Then  (2.85)  can  be  compactly  rewritten  as 


Ax  =  *(C  n)AC-  (2.86) 

It  is  desired  to  compute  AC  starting  from  the  knowledge  of  Cnixn  and  the 
measurement  of  xm.  Since  (2.86)  constitutes  a  system  of  m  equations  into 
4 n  unknowns  with  m  <  4 n,  a  sufficient  number  of  end-effector  pose  measure¬ 
ments  has  to  be  performed  so  as  to  obtain  a  system  of  at  least  4n  equations. 
Therefore,  if  measurements  are  made  for  a  number  of  l  poses,  (2.86)  yields 


Ax  = 


- 1 

tx 
•  B 

_ 1 

<2V 

.  Axi  _ 

AC 


&AC- 


(2.87) 


13 


These  matrices  are  the  Jacobians  of  the  transformations  between  the  parameter 
space  and  the  operational  space. 
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As  regards  the  nominal  values  of  the  parameters  needed  for  the  computation 
of  the  matrices  it  should  be  observed  that  the  geometric  parameters  are 
constant  whereas  the  joint  variables  depend  on  the  manipulator  configuration 
at  pose  i. 

In  order  to  avoid  ill-conditioning  of  matrix  it  is  advisable  to  choose  l 
so  that  Im  4n  and  then  solve  (2.87)  with  a  least-squares  technique;  in  this 
case  the  solution  is  of  the  form 

AC  =  (i '>T^)~1^T  Ax  (2.88) 

where  (<?>T<P)  1<I,T  is  the  left,  pseudo-inverse  matrix  of  #.14  By  computing  <!> 
with  the  nominal  values  of  the  parameters  Cn,  the  first  parameter  estimate  is 
given  by 

C'  =  C„  +  (2.89) 

This  is  a  nonlinear  parameter  estimate  problem  and,  as  such,  the  procedure 
should  be  iterated  until  AC  converges  within  a  given  threshold.  At  each  itera¬ 
tion,  the  calibration  matrix  ^  is  to  be  updated  with  the  parameter  estimates 
C'  obtained  via  (2.89)  at  the  previous  iteration.  In  a  similar  manner,  the  de¬ 
viation  Ax  is  to  be  computed  as  the  difference  between  the  measured  values 
for  the  l  end-effector  poses  and  the  corresponding  poses  computed  by  the  di¬ 
rect  kinematics  function  with  the  values  of  the  parameters  at  the  previous 
iteration.  As  a  result  of  the  kinematic  calibration  procedure,  more  accurate 
estimates  of  the  real  manipulator  geometric  parameters  as  well  as  possible 
corrections  to  make  on  the  joint  transducers  measurements  are  obtained. 

Kinematic  calibration  is  an  operation  that  is  performed  by  the  robot  manu¬ 
facturer  to  guarantee  the  accuracy  reported  in  the  data  sheet.  There  is  another 
kind  of  calibration  that  is  performed  by  the  robot  user  which  is  needed  for  the 
measurement  system  start-up  to  guarantee  that  the  position  transducers  data 
are  consistent  with  the  attained  manipulator  posture.  For  instance,  in  the 
case  of  incremental  (nonabsolute)  position  transducers,  such  calibration  con¬ 
sists  of  taking  the  mechanical  structure  into  a  given  reference  posture  (home) 
and  initializing  the  position  transducers  with  the  values  at  that  posture. 


2.12  Inverse  Kinematics  Problem 

The  direct  kinematics  equation,  either  in  the  form  (2.50)  or  in  the  form  (2.82), 
establishes  the  functional  relationship  between  the  joint  variables  and  the  end- 
effector  position  and  orientation.  The  inverse  kinematics  problem  consists  of 
the  determination  of  the  joint  variables  corresponding  to  a  given  end-effector 
position  and  orientation.  The  solution  to  this  problem  is  of  fundamental  im¬ 
portance  in  order  to  transform  the  motion  specifications,  assigned  to  the  end- 
effector  in  the  operational  space,  into  the  corresponding  joint  space  motions 
that  allow  execution  of  the  desired  motion. 

14  See  Sect.  A. 7  for  the  definition  of  the  pseudo-inverse  of  a  matrix. 
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As  regards  the  direct  kinematics  equation  in  (2.50),  the  end-effector  po¬ 
sition  and  rotation  matrix  are  computed  in  a  unique  manner,  once  the  joint 
variables  are  known15.  On  the  other  hand,  the  inverse  kinematics  problem  is 
much  more  complex  for  the  following  reasons: 

•  The  equations  to  solve  are  in  general  nonlinear,  and  thus  it  is  not  always 
possible  to  find  a  closed-form,  solution. 

•  Multiple  solutions  may  exist. 

•  Infinite  solutions  may  exist,  e.g.,  in  the  case  of  a  kinematically  redundant 
manipulator. 

•  There  might  be  no  admissible  solutions,  in  view  of  the  manipulator  kine¬ 
matic  structure. 

The  existence  of  solutions  is  guaranteed  only  if  the  given  end-effector  position 
and  orientation  belong  to  the  manipulator  dexterous  workspace. 

On  the  other  hand,  the  problem  of  multiple  solutions  depends  not  only  on 
the  number  of  DOFs  but  also  on  the  number  of  non-null  DH  parameters;  in 
general,  the  greater  the  number  of  non-null  parameters,  the  greater  the  num¬ 
ber  of  admissible  solutions.  For  a  six-DOF  manipulator  without  mechanical 
joint  limits,  there  are  in  general  up  to  16  admissible  solutions.  Such  occur¬ 
rence  demands  some  criterion  to  choose  among  admissible  solutions  (e.g.,  the 
elbow-up/elbow-down  case  of  Example  2.6).  The  existence  of  mechanical  joint 
limits  may  eventually  reduce  the  number  of  admissible  multiple  solutions  for 
the  real  structure. 

Computation  of  closed- form  solutions  requires  either  algebraic  intuition  to 
find  those  significant  equations  containing  the  unknowns  or  geometric  intu¬ 
ition  to  find  those  significant  points  on  the  structure  with  respect  to  which 
it  is  convenient  to  express  position  and/or  orientation  as  a  function  of  a  re¬ 
duced  number  of  unknowns.  The  following  examples  will  point  out  the  ability 
required  to  an  inverse  kinematics  problem  solver.  On  the  other  hand,  in  all 
those  cases  when  there  are  no  —  or  it  is  difficult  to  find  —  closed-form  so¬ 
lutions,  it  might  be  appropriate  to  resort  to  numerical  solution  techniques ; 
these  clearly  have  the  advantage  of  being  applicable  to  any  kinematic  struc¬ 
ture,  but  in  general  they  do  not  allow  computation  of  all  admissible  solutions. 
In  the  following  chapter,  it  will  be  shown  how  suitable  algorithms  utilizing 
the  manipulator  Jacobian  can  be  employed  to  solve  the  inverse  kinematics 
problem. 

2.12.1  Solution  of  Three-link  Planar  Arm 

Consider  the  arm  shown  in  Fig.  2.20  whose  direct  kinematics  was  given 
in  (2.63).  It  is  desired  to  find  the  joint  variables  i?i,  $2,  $3  corresponding 
to  a  given  end-effector  position  and  orientation. 

15  In  general,  this  cannot  be  said  for  (2.82)  too,  since  the  Euler  angles  are  not 
uniquely  defined. 
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As  already  pointed  out,  it  is  convenient  to  specify  position  and  orientation 
in  terms  of  a  minimal  number  of  parameters:  the  two  coordinates  px,  py  and 
the  angle  <j>  with  axis  xo,  in  this  case.  Hence,  it  is  possible  to  refer  to  the  direct 
kinematics  equation  in  the  form  (2.83). 

A  first  algebraic  solution  technique  is  illustrated  below.  Having  specified 
the  orientation,  the  relation 


</>  =  01  +  +  03  (2.90) 

is  one  of  the  equations  of  the  system  to  solve16.  From  (2.63)  the  following 
equations  can  be  obtained: 


PWx  =  Px~  a3C0  =  cqci  +  a2Ci2  (2-91) 

PWy  =  Py  -  CL^S^  =  aiSi  +  a2si2  (2.92) 

which  describe  the  position  of  point  W,  i.e.,  the  origin  of  Frame  2;  this  depends 
only  on  the  first  two  angles  •&\  and  d2.  Squaring  and  summing  (2.91),  (2.92) 
yields 

P\Vx  A  P\Vy  =  ai  A  a2  A  2aia2c2 

from  which 

Pwx  +  Pwv  -ai~a2 

C2  =  - TT* - • 

2aia2 

The  existence  of  a  solution  obviously  imposes  that  —  1  <  c2  <  1,  otherwise 
the  given  point  would  be  outside  the  arm  reachable  workspace.  Then,  set 

s2  =  ±yj  1  -  cl, 

where  the  positive  sign  is  relative  to  the  elbow-down  posture  and  the  negative 
sign  to  the  elbow-up  posture.  Hence,  the  angle  $2  can  be  computed  as 

t92  =  Atan2(s2,  c2). 

Having  determined  ,d2,  the  angle  t)\  can  be  found  as  follows.  Substituting 
$2  into  (2.91),  (2.92)  yields  an  algebraic  system  of  two  equations  in  the  two 
unknowns  si  and  ci,  whose  solution  is 

(ai  +  a2C2)pwy  —  a2s2pwx 

P\Vx  +  Pwy 

(oq  +  a2c2)pwx  +  a2s2pwv 
Pwx  +  Pwy 

In  analogy  to  the  above,  it  is 


si  = 

ci  = 


=  Atan2(si,  Ci). 


16 


If  0  is  not  specified,  then  the  arm  is  redundant  and  there  exist  infinite  solutions 
to  the  inverse  kinematics  problem. 
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Fig.  2.31.  Admissible  postures  for  a  two-link  planar  arm 


In  the  case  when  S2  =  0,  it  is  obviously  1)2  =  0, 7r;  as  will  be  shown  in  the 
following,  in  such  a  posture  the  manipulator  is  at  a  kinematic  singularity .  Yet, 
the  angle  1?  1  can  be  determined  uniquely,  unless  a±  =  <22  and  it  is  required 
PWx  =  Vwy  =  0. 

Finally,  the  angle  $3  is  found  from  (2.90)  as 

=  <t>  -  $1  —  1) 2- 

An  alternative  geometric  solution  technique  is  presented  below.  As  above, 
the  orientation  angle  is  given  as  in  (2.90)  and  the  coordinates  of  the  origin 
of  Frame  2  are  computed  as  in  (2.91),  (2.92).  The  application  of  the  cosine 
theorem  to  the  triangle  formed  by  links  a\,  02  and  the  segment  connecting 
points  W  and  O  gives 

PWx  +  Pwy  =  0,1  +  &2  —  2dia2  COS  (lT  —  $2); 

the  two  admissible  configurations  of  the  triangle  are  shown  in  Fig.  2.31.  Ob¬ 
serving  that  cos  (n  —  $ 2)  =  —cos  $2  leads  to 

Pwx  +  Pwy  -  ai  ~  a2 

c2  =  - 0  - • 

la\a,2 

For  the  existence  of  the  triangle,  it  must  be  \jp%/x  +  Pwy  —  01  +  02-  This 
condition  is  not  satisfied  when  the  given  point  is  outside  the  arm  reachable 
workspace.  Then,  under  the  assumption  of  admissible  solutions,  it  is 

$2  =  ±cos_1(c2); 

the  elbow-up  posture  is  obtained  for  $2  G  ( — 7r,  0)  while  the  elbow-down  pos¬ 
ture  is  obtained  for  i}2  G  (0, 7r). 

To  find  i?!  consider  the  angles  a  and  (3  in  Fig.  2.31.  Notice  that  the  deter¬ 
mination  of  a  depends  on  the  sign  of  pwx  and  pwy\  then,  it  is  necessary  to 
compute  a  as 


a  =  Atan2  (pWy,pWx). 
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To  compute  /?,  applying  again  the  cosine  theorem  yields 

c0\Jpwx+Pwv  =  °i  +  °2C2 

and  resorting  to  the  expression  of  C2  given  above  leads  to 

.  _i  (  Pwx+Pwy  +  ai~a2 

p  =  cos  -  = — 

\  2a  1  Y 

with  (3  £  (0,7r)  so  as  to  preserve  the  existence  of  triangles.  Then,  it  is 

=  a  ±  (3, 

where  the  positive  sign  holds  for  i) 2  <  0  and  the  negative  sign  for  $2  >  0. 
Finally,  $3  is  computed  from  (2.90). 

It  is  worth  noticing  that,  in  view  of  the  substantial  equivalence  between 
the  two-link  planar  arm  and  the  parallelogram  arm,  the  above  techniques  can 
be  formally  applied  to  solve  the  inverse  kinematics  of  the  arm  in  Sect.  2.9.2. 

2.12.2  Solution  of  Manipulators  with  Spherical  Wrist 

Most  of  the  existing  manipulators  are  kinematically  simple,  since  they  are 
typically  formed  by  an  arm,  of  the  kind  presented  above,  and  a  spherical  wrist; 
see  the  manipulators  in  Sects.  2. 9. 6-2. 9. 8.  This  choice  is  partly  motivated  by 
the  difficulty  to  find  solutions  to  the  inverse  kinematics  problem  in  the  general 
case.  In  particular,  a  si:r-DOF  kinematic  structure  has  closed-form  inverse 
kinematics  solutions  if: 

•  three  consecutive  revolute  joint  axes  intersect  at  a  common  point,  like  for 
the  spherical  wrist; 

•  three  consecutive  revolute  joint  axes  are  parallel. 

In  any  case,  algebraic  or  geometric  intuition  is  required  to  obtain  closed-form 
solutions. 

Inspired  by  the  previous  solution  to  a  three-link  planar  arm,  a  suitable 
point  along  the  structure  can  be  found  whose  position  can  be  expressed  both  as 
a  function  of  the  given  end-effector  position  and  orientation  and  as  a  function 
of  a  reduced  number  of  joint  variables.  This  is  equivalent  to  articulating  the 
inverse  kinematics  problem  into  two  subproblems,  since  the  solution  for  the 
position  is  decoupled  from  that  for  the  orientation. 

For  a  manipulator  with  spherical  wrist,  the  natural  choice  is  to  locate  such 
point  W  at  the  intersection  of  the  three  terminal  revolute  axes  (Fig.  2.32).  In 
fact,  once  the  end-effector  position  and  orientation  are  specified  in  terms  of 
pe  and  Re  =  [ne  se  ae],  the  wrist  position  can  be  found  as 


Pw  =Pe~ 


(2.93) 
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which  is  a  function  of  the  sole  joint  variables  that  determine  the  arm  posi¬ 
tion1'.  Hence,  in  the  case  of  a  (nonredundant)  three-DOF  arm,  the  inverse 
kinematics  can  be  solved  according  to  the  following  steps: 

•  Compute  the  wrist  position  Pw(qi,  92,  93)  as  in  (2.93). 

•  Solve  inverse  kinematics  for  (91,  <72 ,  <73)- 

•  Compute  R°3  (q± ,  q2 ,  q3 )  ■ 

•  Compute  Hg($4,  $5,  &$)  =  R3TR. 

•  Solve  inverse  kinematics  for  orientation  ($4,  $5,  i?e). 

Therefore,  on  the  basis  of  this  kinematic  decoupling,  it  is  possible  to  solve 
the  inverse  kinematics  for  the  arm  separately  from  the  inverse  kinematics 
for  the  spherical  wrist.  Below  are  presented  the  solutions  for  two  typical  arms 
(spherical  and  anthropomorphic)  as  well  as  the  solution  for  the  spherical  wrist. 

2.12.3  Solution  of  Spherical  Arm 

Consider  the  spherical  arm  shown  in  Fig.  2.22,  whose  direct  kinematics  was 
given  in  (2.65).  It  is  desired  to  find  the  joint  variables  t?i,  i9 2,  d$  corresponding 
to  a  given  end-effector  position  pw. 

In  order  to  separate  the  variables  on  which  pw  depends,  it  is  convenient  to 
express  the  position  of  pw  with  respect  to  Frame  1;  then,  consider  the  matrix 
equation 

(A?)-1^  =  A'Al 


17  Note  that  the  same  reasoning  was  implicitly  adopted  in  Sect.  2.12.1  for  the  three- 
link  planar  arm;  pw  described  the  one-DOF  wrist  position  for  the  two-DOF  arm 
obtained  by  considering  only  the  first  two  links. 
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Equating  the  first  three  elements  of  the  fourth  columns  of  the  matrices  on 
both  sides  yields 


PWxCl  +PWyS  1 

d3S2 

~PWz 

= 

—d3C2 

_  -PWxSl  +  PWyCl  _ 

02 

(2.94) 


which  depends  only  on  02  and  0.3-  To  solve  this  equation,  set 

0i 

t  =  tan  — 


so  that 


Substituting  this 
of  (2.94)  gives 

whose  solution  is 


1  -  t2  2 1 

Cl  ~  1  +  t2  Sl  “  1  +  t2' 

equation  in  the  third  component  on  the  left-hand  side 

(02  +  PWy)t2  +  2p\Vxt  +  02  —  Pwy  =  0) 

PWx  ±  Pwx  +  P\Vy  ~~  ^2 
02  +  PWy 


The  two  solutions  correspond  to  two  different  postures.  Hence,  it  is 


0i  =  2Atan2  (~pWx  ±  \Jpwx+Pwy  ~  dh  d2  +  pwv )  • 

Once  $1  is  known,  squaring  and  summing  the  first  two  components  of  (2.94) 
yields 

03  =  \J  {PWxCl  +  PWySl)2  +  Pwz, 

where  only  the  solution  with  03  >  0  has  been  considered.  Note  that  the  same 
value  of  03  corresponds  to  both  solutions  for  Finally,  if  03  ^  0,  from  the 
first  two  components  of  (2.94)  it  is 

PWxCl  +  PWySl  _  d3S2 

PW  z  —  03  C2  ’ 


from  which 


02  =  Mai\2{pWxCi  +  PWySl, PWz)- 


Notice  that,  if  d3  =  0,  then  02  cannot  be  uniquely  determined. 


2.12.4  Solution  of  Anthropomorphic  Arm 

Consider  the  anthropomorphic  arm  shown  in  Fig.  2.23.  It  is  desired  to  find 
the  joint  variables  0i,  02,  03  corresponding  to  a  given  end-effector  position 
pw.  Notice  that  the  direct  kinematics  for  pw  is  expressed  by  (2.66)  which  can 
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be  obtained  from  (2.70)  by  setting  d6  =  0,  $4  =  a 3  and  replacing  i93  with  the 
angle  $3  +  7t/2  because  of  the  misalignment  of  the  Frames  3  for  the  structures 
in  Fig.  2.23  and  in  Fig.  2.26,  respectively.  Hence,  it  follows 

PWx  =  ci(a2c2  +  a3c23)  (2.95) 

Pwy  =  si(a2c2  +  a3c23)  (2.96) 

PWz  =  «2S2  +  CL3S23-  (2.97) 


Proceeding  as  in  the  case  of  the  two-link  planar  arm,  it  is  worth  squaring 
and  summing  (2.95)-(2.97)  yielding 

Pwx  +  Pwy  +  Pwz  =  u2  +  a3  +  2  a2a3c3 


from  which 


C3  = 


P\Vx 


-P\Vy 


'  P\Vz 


—  as  —  a; 


2  a2a3 


(2.98) 


where  the  admissibility  of  the  solution  obviously  requires  that  —  1  <  c3  <  1, 
or  equivalently  |a2  —  a3|  <  ^Pwx  +  p\ry  +  Pwz  ^  a2  +  a3,  otherwise  the  wrist 
point  is  outside  the  reachable  workspace  of  the  manipulator.  Hence  it  is 


s3  =  ±\A-c§ 


and  thus 


r?3  =  Atan2(s3,c3) 

giving  the  two  solutions,  according  to  the  sign  of  S3, 


$3,1  €  [— 7T,  7r] 
$3,11  =  -$3  ,/• 


(2.99) 


(2.100) 

(2.101) 


Having  determined  $3,  it  is  possible  to  compute  $2  as  follows.  Squaring 
and  summing  (2.95),  (2.96)  gives 

P\Vx  +  P\Vy  =  (a2C2  +  a3c23)2 

from  which 

a2c2  +  a3c23  =  ±y ^Pwx  +P\vy-  (2.102) 

The  system  of  the  two  Eqs.  (2.102),  (2.97),  for  each  of  the  solutions  (2.100), 
(2.101),  admits  the  solutions: 


±  \Jp\Vx  +  Pwy(a2  +  a3°3)  +  PWZa3S3 

a2  +  a3  +  2a2a3c3 

Pwz{a2  +  a3c3 )  =F  ^Pwx+Pwva 3S3 
a 2  T  u3  -I-  2a2a3c3 


(2.103) 

(2.104) 
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From  (2.103),  (2.104)  it  follows 

$2  =  Atan2(s2,  c2) 


which  gives  the  four  solutions  for  $2,  according  to  the  sign  of  S3  in  (2.99): 


tf2j  =  Atan2  ((a2  +  a3c3)pWz  -  a3sj \Jpwx+Pwy, 

(a2  +  a3c3)yJp^Vx+p^Vy  +  a3s3pWz^j  (2.105) 
^2.11  =  Atan2  ((a2  +  a3c3)pWz  +  a3s^  \Jpwx+Pwy , 

-(a2  +  u3c3)^p\Vx+p1Wy  +  a3s%pWz^  (2.106) 

corresponding  to  S3"  =  \/ 1  —  c3 ,  and 

$2,111  =  Atan2  ((a2  +  a3c3)pWz  -  a3Sg  \Jpwx+P\vy’ 

(a2  +  a3c3)sJp\Vx+p1Wy  +  a3s3pw z)  (2.107) 
$2, iv  =  Atan2  ((a2  +  a3c3)pWz  +  a3Sg  \Jpwx+P\vV’ 

-(a2  +  a3c3)  yjp^^,  +  p^y  +  a3s3  pwz^j  (2.108) 

corresponding  to  s3  =  —  \/l  —  c3. 

Finally,  to  compute  $1,  it  is  sufficient  to  rewrite  (2.95),  (2.96),  using 

(2.102),  as 


pWx  =  ±ci^p2Wx+p^y 

PWy  =  ±S1  \]pwx  +  P\Vy 

which,  once  solved,  gives  the  two  solutions: 

$1,1  =  ^ai\2{pwv,Pwx) 

$1,11  =  Atan2(—  pwy,  ~Pwx )• 

Notice  that  (2.110)  gives18 


f  Atai \2{pWy,pWx)  -  7T  pWy  >  0 

1/1  II  =  S 

l  Atan2 (pwy,Pwx)  +  tt  pwv  <  0. 
18  It  is  easy  to  show  that  Atan2(— y,  — x )  =  — Atan2(y,  —x)  and 


Atan2(y,  —  x) 


7r  —  Atan2(y,  x)  y  >  0 
— 7r  —  Atan2(i/,  x)  y  <  0. 


(2.109) 

(2.110) 
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Fig.  2.33.  The  four  configurations  of  an  anthropomorphic  arm  compatible  with  a 
given  wrist  position 

As  can  be  recognized,  there  exist  four  solutions  according  to  the  values  of 
# 3  in  (2.100),  (2.101),  #2  in  (2.105)-(2.108)  and  #i  in  (2.109),  (2.110): 

(#1,1)  $2,1)  $3, i)  (#1,1)  #2, III)  #3, II)  (#1,11)  $2,11)  #3, i)  (#1,11)  #2, IV)  #3, II)) 

which  are  illustrated  in  Fig.  2.33:  shoulder-right/elbow-up,  shoulder-left/elbow- 
up,  shoulder-right/elbow-down,  shoulder-left/elbow-down;  obviously,  the  fore¬ 
arm  orientation  is  different  for  the  two  pairs  of  solutions. 

Notice  finally  how  it  is  possible  to  find  the  solutions  only  if  at  least 

PWx  7^  0  or  pWy  ^  0. 

In  the  case  Pwx  =  PWy  =  0,  an  infinity  of  solutions  is  obtained,  since  it  is 
possible  to  determine  the  joint  variables  $2  and  $3  independently  of  the  value 
of  $1 ;  in  the  following,  it  will  be  seen  that  the  arm  in  such  configuration  is 
kinematically  singular  (see  Problem  2.18). 

2.12.5  Solution  of  Spherical  Wrist 

Consider  the  spherical  wrist  shown  in  Fig.  2.24,  whose  direct  kinematics  was 
given  in  (2.67).  It  is  desired  to  find  the  joint  variables  $4,  $5,  $6  corresponding 
to  a  given  end-effector  orientation  R g.  As  previously  pointed  out,  these  angles 
constitute  a  set  of  Euler  angles  ZYZ  with  respect  to  Frame  3.  Hence,  having 
computed  the  rotation  matrix 
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from  its  expression  in  terms  of  the  joint  variables  in  (2.67),  it  is  possible  to 
compute  the  solutions  directly  as  in  (2.19),  (2.20),  i.e., 


$4  =  Atan2(ay,  a^) 

i95  =  Atan2^(a|)2  +  (a|)2,a^  (2.111) 

=  Atan2(s^,  —  n3z) 

for  ^5  G  (0,7r),  and 

$4  =  Atan2(— a,y,  —  a%.) 

i95  =  Atan2(—  ^/(a|)2  +  (a^)2,a^  (2.112) 

$6  =  Atan2(— s^,  n^) 

for  i9 5  G  (— 7r,  0). 
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Problems 

2.1.  Find  the  rotation  matrix  corresponding  to  the  set  of  Euler  angles  ZXZ. 

2.2.  Discuss  the  inverse  solution  for  the  Euler  angles  ZYZ  in  the  case  s#  =  0. 
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Fig.  2.34.  Four-link  closed-chain  planar  arm  with  prismatic  joint 


2.3.  Discuss  the  inverse  solution  for  the  Roll-Pitch-Yaw  angles  in  the  case 
ca  =  0. 

2.4.  Verify  that  the  rotation  matrix  corresponding  to  the  rotation  by  an  angle 
about  an  arbitrary  axis  is  given  by  (2.25). 

2.5.  Prove  that  the  angle  and  the  unit  vector  of  the  axis  corresponding  to  a 
rotation  matrix  are  given  by  (2.27),  (2.28).  Find  inverse  formulae  in  the  case 
of  sin  i)  =  0. 

2.6.  Verify  that  the  rotation  matrix  corresponding  to  the  unit  quaternion  is 
given  by  (2.33). 

2.7.  Prove  that  the  unit  quaternion  is  invariant  with  respect  to  the  rotation 
matrix  and  its  transpose,  i.e.,  R(r),  e)e  =  RT (77,  e)e  =  e. 

2.8.  Prove  that  the  unit  quaternion  corresponding  to  a  rotation  matrix  is 
given  by  (2.34),  (2.35). 

2.9.  Prove  that  the  quaternion  product  is  expressed  by  (2.37). 

2.10.  By  applying  the  rules  for  inverting  a  block-partitioned  matrix,  prove 
that  matrix  A J  is  given  by  (2.45). 

2.11.  Find  the  direct  kinematics  equation  of  the  four-link  closed-chain  planar 
arm  in  Fig.  2.34,  where  the  two  links  connected  by  the  prismatic  joint  are 
orthogonal  to  each  other. 

2.12.  Find  the  direct  kinematics  equation  for  the  cylindrical  arm  in  Fig.  2.35. 

2.13.  Find  the  direct  kinematics  equation  for  the  SCARA  manipulator  in 
Fig.  2.36. 

2.14.  Find  the  complete  direct  kinematics  equation  for  the  humanoid  manip¬ 
ulator  in  Fig.  2.28. 
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2.15.  For  the  set  of  minimal  representations  of  orientation  <j>,  define  the  sum 
operation  in  terms  of  the  composition  of  rotations.  By  means  of  an  example, 
show  that  the  commutative  property  does  not  hold  for  that  operation. 

2.16.  Consider  the  elementary  rotations  about  coordinate  axes  given  by  in¬ 
finitesimal  angles.  Show  that  the  rotation  resulting  from  any  two  elementary 
rotations  does  not  depend  on  the  order  of  rotations.  [Hint:  for  an  infinitesimal 
angle  d<j>,  approximate  cos  (d<j>)  «  1  and  sin  (d<j>)  ss  d(f>  ...].  Further,  define 
R(d<j)x,  d(j)v,  d(j>z)  =  Rx(d(f>x)Ry(d(f>y)Rz(d(f>z);  show  that 

R{dcj)Xld(j)y,  d(f)z)R{d(!),x ,  d<j>'y,  d4>'z)  =  R(dc/>x  +  d<f/x ,  d(f>y  +  d<j>'y,  d<j>z  +  d$z). 

2.17.  Draw  the  workspace  of  the  three-link  planar  arm  in  Fig.  2.20  with  the 
data: 

di  =  0.5  Oj2  =  0.3  d3  =  0.2 
— 7r/3  <  q\  <  7r/3  —  27r/3  <  q2  <  2n/3  —  7t/2  <  <  7r/2. 

2.18.  With  reference  to  the  inverse  kinematics  of  the  anthropomorphic  arm 
in  Sect.  2.12.4,  discuss  the  number  of  solutions  in  the  singular  cases  of  S3  =  0 
and  pWx  =  pWy  =  0. 

2.19.  Solve  the  inverse  kinematics  for  the  cylindrical  arm  in  Fig.  2.35. 

2.20.  Solve  the  inverse  kinematics  for  the  SCARA  manipulator  in  Fig.  2.36. 
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Differential  Kinematics  and  Statics 


In  the  previous  chapter,  direct  and  inverse  kinematics  equations  establishing 
the  relationship  between  the  joint  variables  and  the  end-effector  pose  were 
derived.  In  this  chapter,  differential  kinematics  is  presented  which  gives  the 
relationship  between  the  joint  velocities  and  the  corresponding  end-effector 
linear  and  angular  velocity.  This  mapping  is  described  by  a  matrix,  termed 
geometric  Jacobian,  which  depends  on  the  manipulator  configuration.  Alter¬ 
natively,  if  the  end-effector  pose  is  expressed  with  reference  to  a  minimal 
representation  in  the  operational  space,  it  is  possible  to  compute  the  Jaco¬ 
bian  matrix  via  differentiation  of  the  direct  kinematics  function  with  respect 
to  the  joint  variables.  The  resulting  Jacobian,  termed  analytical  Jacobian ,  in 
general  differs  from  the  geometric  one.  The  Jacobian  constitutes  one  of  the 
most  important  tools  for  manipulator  characterization;  in  fact,  it  is  useful  for 
finding  singularities,  analyzing  redundancy,  determining  inverse  kinematics 
algorithms,  describing  the  mapping  between  forces  applied  to  the  end-effector 
and  resulting  torques  at  the  joints  ( statics )  and,  as  will  be  seen  in  the  follow¬ 
ing  chapters,  deriving  dynamic  equations  of  motion  and  designing  operational 
space  control  schemes.  Finally,  the  kineto- statics  duality  concept  is  illustrated, 
which  is  at  the  basis  of  the  definition  of  velocity  and  force  manipulability  el¬ 
lipsoids  . 


3.1  Geometric  Jacobian 


Consider  an  n-DOF  manipulator.  The  direct  kinematics  equation  can  be  writ¬ 
ten  in  the  form 


Re  (q)  Pe(q ) 

0T  1  . 


(3.1) 


where  q  =  [qi  ...  qn]T  is  the  vector  of  joint  variables.  Both  end-effector 
position  and  orientation  vary  as  q  varies. 
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The  goal  of  the  differential  kinematics  is  to  find  the  relationship  between 
the  joint  velocities  and  the  end-effector  linear  and  angular  velocities.  In  other 
words,  it  is  desired  to  express  the  end-effector  linear  velocity  pe  and  angular 
velocity  we  as  a  function  of  the  joint  velocities  q.  As  will  be  seen  afterwards, 
the  sought  relations  are  both  linear  in  the  joint  velocities,  i.e., 

Pe  =  Jp(tl)Q  (3-2) 


u3e  =  J0{q)q •  (3.3) 

In  (3.2)  JP  is  the  (3  x  n)  matrix  relating  the  contribution  of  the  joint  veloc¬ 
ities  q  to  the  end-effector  linear  velocity  pe,  while  in  (3.3)  Jo  is  the  (3  x  n) 
matrix  relating  the  contribution  of  the  joint  velocities  q  to  the  end-effector 
angular  velocity  u)e.  In  compact  form,  (3.2),  (3.3)  can  be  written  as 


ve 


Pe 

UJ  e 


J{q)q 


(3.4) 


which  represents  the  manipulator  differential  kinematics  equation.  The  (6  x  n) 
matrix  J  is  the  manipulator  geometric  Jacobian 


J  = 


Jp 

Jo 


(3.5) 


which  in  general  is  a  function  of  the  joint  variables. 

In  order  to  compute  the  geometric  Jacobian,  it  is  worth  recalling  a  number 
of  properties  of  rotation  matrices  and  some  important  results  of  rigid  body 
kinematics. 


3.1.1  Derivative  of  a  Rotation  Matrix 

The  manipulator  direct  kinematics  equation  in  (3.1)  describes  the  end-effector 
pose,  as  a  function  of  the  joint  variables,  in  terms  of  a  position  vector  and  a 
rotation  matrix.  Since  the  aim  is  to  characterize  the  end-effector  linear  and 
angular  velocities,  it  is  worth  considering  first  the  derivative  of  a  rotation 
matrix  with  respect  to  time. 

Consider  a  time- varying  rotation  matrix  R  =  R(t).  In  view  of  the  orthog¬ 
onality  of  R ,  one  has  the  relation 

R{t)RT(t)  =  I 

which,  differentiated  with  respect  to  time,  gives  the  identity 
R{t)RT(t)  +  R{t)RT(t)  =  O. 


Set 


S(t)  =  R(t)RT(t)-, 


(3.6) 


3.1  Geometric  Jacobian 
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the  (3  x  3)  matrix  S  is  skew-symmetric  since 

S(t)  +  ST(t)  =  O.  (3.7) 

Postmultiplying  both  sides  of  (3.6)  by  R(t)  gives 

R(t)  =  S(t)R(t)  (3.8) 

that  allows  the  time  derivative  of  R(t)  to  be  expressed  as  a  function  of  R{t) 
itself. 

Equation  (3.8)  relates  the  rotation  matrix  R  to  its  derivative  by  means 
of  the  skew-symmetric  operator  S  and  has  a  meaningful  physical  interpreta¬ 
tion.  Consider  a  constant  vector  p'  and  the  vector  p(t)  =  R(t)p' .  The  time 
derivative  of  p(t)  is 

P(t)  =  R{t)p', 

which,  in  view  of  (3.8),  can  be  written  as 

P(t)  =  S(t)R(t)p' . 

If  the  vector  w(t)  denotes  the  angular  velocity  of  frame  R(t)  with  respect  to 
the  reference  frame  at  time  t,  it  is  known  from  mechanics  that 

p(t)  =  u>(t)  x  R(t)p' . 

Therefore,  the  matrix  operator  S(t)  describes  the  vector  product  between  the 
vector  u>  and  the  vector  R(t)p' .  The  matrix  S(t)  is  so  that  its  symmetric 
elements  with  respect  to  the  main  diagonal  represent  the  components  of  the 
vector  u> (t)  =  [ u>x  uiv  loz]t  in  the  form 

0  ~U)Z  LOy 

S  =  uz  0  —ux  ,  (3.9) 

LOy  LOx  0 

which  justifies  the  expression  S(t)  =  Hence,  (3.8)  can  be  rewritten 

as 

R  =  S(lo)R.  (3.10) 

Furthermore,  if  R  denotes  a  rotation  matrix,  it  can  be  shown  that  the 
following  relation  holds: 

RS{co)RT  =  S(Rlo)  (3.11) 

which  will  be  useful  later  (see  Problem  3.1). 
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Example  3.1 

Consider  the  elementary  rotation  matrix  about  axis  a  given  in  (2.6).  If  a  is  a  function 
of  time,  by  computing  the  time  derivative  of  Rz(a(t)),  (3.6)  becomes 


S{t)  = 


— asina 
a  cos  a 
0 


—a  cos  a 
—a  sin  a 
0 


cos  a 

sina 

O' 

—sin  a 

cos  a 

0 

0 

0 

1. 

0  —a  0 

a  0  0 

0  0  0 


According  to  (3.9),  it  is 


u>  =  [0  0  a  }T 


that  expresses  the  angular  velocity  of  the  frame  about  axis  2. 


With  reference  to  Fig.  2.11,  consider  the  coordinate  transformation  of  a 
point  P  from  Frame  1  to  Frame  0;  in  view  of  (2.38),  this  is  given  by 

p°  =  o°1  +  R°1p1.  (3.12) 

Differentiating  (3.12)  with  respect  to  time  gives 

p°  =  oj  +  Rip1  +  RiP1;  (3.13) 

utilizing  the  expression  of  the  derivative  of  a  rotation  matrix  (3.8)  and  speci¬ 
fying  the  dependence  on  the  angular  velocity  gives 

P°  =  b°  +  R^p1  +  Sioj^R^p1. 

Further,  denoting  the  vector  R^p1  by  rfi  it  is 

p°  =  6?  +  R^p1  +  w?  x  r°  (3.14) 

which  is  the  known  form  of  the  velocity  composition  rule. 

Notice  that,  if  p1  is  fixed  in  Frame  1,  then  it  is 

p°  =  6?  +  u}\  x  r?  (3.15) 

since  p1  =  0. 

3.1.2  Link  Velocities 

Consider  the  generic  Link  i  of  a  manipulator  with  an  open  kinematic  chain. 
According  to  the  Denavit-Hartenberg  convention  adopted  in  the  previous 
chapter,  Link  i  connects  Joints  i  and  i  +  1;  Frame  i  is  attached  to  Link  i 
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Fig.  3.1.  Characterization  of  generic  Link  i  of  a  manipulator 


and  has  origin  along  Joint  i  + 1  axis,  while  Frame  i  —  1  has  origin  along  Joint  i 
axis  (Fig.  3.1). 

Let  and  p{  be  the  position  vectors  of  the  origins  of  Frames  i  —  1  and  i, 
respectively.  Also,  let  rl~_  \  i  denote  the  position  of  the  origin  of  Frame  i  with 
respect  to  Frame  i  —  1  expressed  in  Frame  i  —  1.  According  to  the  coordinate 
transformation  (3.12),  one  can  write1 

Pi  =Pi-i+  Ri-ir]z{ti- 
Then,  by  virtue  of  (3.14),  it  is 

Pi  =  Pi-x  +  Ri-irfzl  i  +  Vi- 1  x  Ri-xrliZ\  t  =  pi_x  +  vz-lti  +  Wj_i  x  M 

(3-16) 

which  gives  the  expression  of  the  linear  velocity  of  Link  i  as  a  function  of  the 
translational  and  rotational  velocities  of  Link  i  —  1.  Note  that  denotes 

the  velocity  of  the  origin  of  Frame  i  with  respect  to  the  origin  of  Frame  i—1. 

Concerning  link  angular  velocity,  it  is  worth  starting  from  the  rotation 
composition 

R,  =  R,  - 1 R,  1; 

from  (3.8),  its  time  derivative  can  be  written  as 

S(ui)Ri  =  S(u>i-i)Ri  +  Ri-xSiv^Rt1  (3.17) 


where  ,  denotes  the  angular  velocity  of  Frame  i  with  respect  to  Frame 
i  —  1  expressed  in  Frame  i  —  1.  From  (2.4),  the  second  term  on  the  right-hand 
side  of  (3.17)  can  be  rewritten  as 


Ri-xS^rUR1-1  =  Ri-iS(vlz\,i)Ri-xRi-xK 


1  Hereafter,  the  indication  of  superscript  ‘0’  is  omitted  for  quantities  referred  to 
Frame  0.  Also,  without  loss  of  generality,  Frame  0  and  Frame  n  are  taken  as  the 
base  frame  and  the  end-effector  frame,  respectively. 


no 
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in  view  of  property  (3.11),  it  is 

Then,  (3.17)  becomes 

S(ui)Ri  =  S(u/i-!)Ri  +  SiR^XlljRi 
leading  to  the  result 

=  Wj_  i  +  Ri- =  Wj- 1  +  (3.18) 

which  gives  the  expression  of  the  angular  velocity  of  Link  i  as  a  function  of 
the  angular  velocities  of  Link  i  —  1  and  of  Link  i  with  respect  to  Link  i  —  1. 

The  relations  (3.16),  (3.18)  attain  different  expressions  depending  on  the 
type  of  Joint  i  ( prismatic  or  revolute). 

Prismatic  joint 

Since  orientation  of  Frame  i  with  respect  to  Frame  i  —  1  does  not  vary  by 
moving  Joint  i,  it  is 

=  0.  (3.19) 

Further,  the  linear  velocity  is 

Vi—  i,i  =  diZi—\  (3.20) 

where  Zj_i  is  the  unit  vector  of  Joint  i  axis.  Hence,  the  expressions  of  angular 
velocity  (3.18)  and  linear  velocity  (3.16)  respectively  become 

LO  i  (3.21) 

Pi  =  Pi_  1  +  diZi- 1  +  U3i  x  ri-hi,  (3.22) 

where  the  relation  u \  =  oJi-i  has  been  exploited  to  derive  (3.22). 


Revolute  joint 

For  the  angular  velocity  it  is  obviously 


—  $iZi— 1) 

(3.23) 

while  for  the  linear  velocity  it  is 

V jj — —  CtJ 2 — 1,2  X  V i  — 

(3.24) 

due  to  the  rotation  of  Frame  i  with  respect  to  Frame  i  —  1  induced  by  the 
motion  of  Joint  i.  Hence,  the  expressions  of  angular  velocity  (3.18)  and  linear 
velocity  (3.16)  respectively  become 

UJi  =  Ui_ !  i 

Pi  =  Pi-l  +  Vi  X  Vi-Iti, 

(3.25) 

(3.26) 

where  (3.18)  has  been  exploited  to  derive  (3.26). 
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Fig.  3.2.  Representation  of  vectors  needed  for  the  computation  of  the  velocity 
contribution  of  a  revolute  joint  to  the  end-effector  linear  velocity 


3.1.3  Jacobian  Computation 

In  order  to  compute  the  Jacobian,  it  is  convenient  to  proceed  separately  for 
the  linear  velocity  and  the  angular  velocity. 

For  the  contribution  to  the  linear  velocity,  the  time  derivative  of  pe(q)  can 
be  written  as 

n  n 

Pe  =  '52-£rQi  =  '52jpiQi-  {3.27) 

i=  1  i—  1 

This  expression  shows  how  pe  can  be  obtained  as  the  sum  of  the  terms  qijPi. 
Each  term  represents  the  contribution  of  the  velocity  of  single  Joint  i  to  the 
end-effector  linear  velocity  when  all  the  other  joints  are  still. 

Therefore,  by  distinguishing  the  case  of  a  prismatic  joint  (g,;  =  d,;)  from 
the  case  of  a  revolute  joint  (g,;  =  i?*),  it  is: 

•  If  Joint  i  is  prismatic,  from  (3.20)  it  is 

QiJpi  =  diZi- i 

and  then 

Jpi  =  zi- 1- 

•  If  Joint  i  is  revolute,  observing  that  the  contribution  to  the  linear  velocity 
is  to  be  computed  with  reference  to  the  origin  of  the  end-effector  frame 
(Fig.  3.2),  it  is 

QiJpi  =  Ui-l'i  X  T-i-pe  =  tiiZi- 1  X  {pe  -Pi_!) 

and  then 

3Pi  =  Zi- 1  x  {Pe-Pi-i). 
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For  the  contribution  to  the  angular  velocity ,  in  view  of  (3.18),  it  is 

n  n 

Me  =  =  ^2,30iqu  (3.28) 

i- 1  i= 1 

where  (3.19)  and  (3.23)  have  been  utilized  to  characterize  the  terms  qiJoii 
and  thus  in  detail: 

•  If  Joint  i  is  prismatic,  from  (3.19)  it  is 

QiJoi  =  0 

and  then 

Jch  =  0. 

•  If  Joint  i  is  revolute,  from  (3.23)  it  is 


and  then 


iiJoi  =  tiiZi-i 
JOi  =  Zi~  1- 


In  summary,  the  Jacobian  in  (3.5)  can  be  partitioned  into  the  (3  x  1) 
column  vectors  jPi  and  j0i  as 


J  = 


3pi 

Op, 

3oi 

Jo, 

(3.29) 


where 


JPi 

JOi 


Zi- 1 

0 

2;_1  X  (pe  -P^) 
Zi- 1 


for  a  prismatic  joint 
for  a  revolute  joint. 


(3.30) 


The  expressions  in  (3.30)  allow  Jacobian  computation  in  a  simple,  systematic 
way  on  the  basis  of  direct  kinematics  relations.  In  fact,  the  vectors  Zj_i,  pe 
and  Pi_i  are  all  functions  of  the  joint  variables.  In  particular: 


•  z.i_ i  is  given  by  the  third  column  of  the  rotation  matrix  R®_i,  i.e., 


zi- i  —  ■  ■  ■  Ri-i(<li-i)zo  (3.31) 

where  Zq  =  [0  0  1]T  allows  the  selection  of  the  third  column. 

•  pe  is  given  by  the  first  three  elements  of  the  fourth  column  of  the  trans¬ 
formation  matrix  T°e,  i.e.,  by  expressing  pe  in  the  (4  x  1)  homogeneous 
form 

pe  =  Al{qi)...Ann-\qn)p0  (3.32) 

where  p0  =  [0  0  0  1]T  allows  the  selection  of  the  fourth  column. 
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•  Pi-i  is  given  by  the  first  three  elements  of  the  fourth  column  of  the  trans¬ 
formation  matrix  T°i_l ,  i.e.,  it  can  be  extracted  from 


Pi-i  —  ^4-1  (<7i)  ■  •  •  Ai-ife-OPo-  (3.33) 


The  above  equations  can  be  conveniently  used  to  compute  the  translational 
and  rotational  velocities  of  any  point  along  the  manipulator  structure,  as  long 
as  the  direct  kinematics  functions  relative  to  that  point  are  known. 

Finally,  notice  that  the  Jacobian  matrix  depends  on  the  frame  in  which 
the  end-effector  velocity  is  expressed.  The  above  equations  allow  computation 
of  the  geometric  Jacobian  with  respect  to  the  base  frame.  If  it  is  desired  to 
represent  the  Jacobian  in  a  different  Frame  u,  it  is  sufficient  to  know  the 
relative  rotation  matrix  Ru.  The  relationship  between  velocities  in  the  two 
frames  is 


'Pe  ' 

Ru 

O 

Pe 

O 

Ru 

UJe 

which,  substituted  in  (3.4),  gives 


'  Pe  ' 

Ru 

O 

O 

Ru 

and  then 


Ju 


Ru  O  1 
O  Ru 


where  Ju  denotes  the  geometric  Jacobian  in  Frame  u. 


(3.34) 


3.2  Jacobian  of  Typical  Manipulator  Structures 

In  the  following,  the  Jacobian  is  computed  for  some  of  the  typical  manipulator 
structures  presented  in  the  previous  chapter. 


3.2.1  Three-link  Planar  Arm 

In  this  case,  from  (3.30)  the  Jacobian  is 

J(q)  = 


Z0  X  (P3  -  Po)  Z!  X  (p3  -  PX)  Z2  X  {p3 
Zo  Z!  Z2 


Computation  of  the  position  vectors  of  the  various  links  gives 


'O' 

diCi 

aqci  +  a2ci2 

0 

Pi  = 

aisi 

P‘2  = 

nisi  +  a2si2 

0 

0 

0 
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Ps  = 


aiCi  +  CI2C12  +  CI3C123 
<2lO  +  CI2S12  +  <23023 
0 


while  computation  of  the  unit  vectors  of  revolute  joint  axes  gives 


zq  =  Zl  =  2:2 


0 

0 

1 


since  they 

are  all  parallel  to  axis  zq  . 

From  (3.29)  it  is 

"  —  CtlSi  —  02S12  —  <23Si23 

— (I2S12  —  <235*123 

-<23Sl2c 

aiCi  +  a2Ci2  +  <230123 

<22<2l2  +  <23023 

<23023 

0 

0 

0 

J  = 

0 

0 

0 

0 

0 

0 

1 

1 

1 

(3.35) 


In  the  Jacobian  (3.35),  only  the  three  non-null  rows  are  relevant  (the  rank  of 
the  matrix  is  at  most  3) ;  these  refer  to  the  two  components  of  linear  velocity 
along  axes  Xq,  y3  and  the  component  of  angular  velocity  about  axis  20  •  This 
result  can  be  derived  by  observing  that  three  DOFs  allow  specification  of  at 
most  three  end-effector  variables;  vz,  u>x,  lov  are  always  null  for  this  kinematic 
structure.  If  orientation  is  of  no  concern,  the  (2  x  3)  Jacobian  for  the  positional 
part  can  be  derived  by  considering  just  the  first  two  rows,  i.e. , 


Jp 


—  aiSi  —  CI2S12  —  <23023  —  <*202  —  <23023  —<235123 

OiCi  +  02C12  +  CI3C123  <2202  +  <23c123  <23023 


(3.36) 


3.2.2  Anthropomorphic  Arm 


In  this  case,  from  (3.30)  the  Jacobian  is 

J  =  \zq  X  (p3  -Po)  z1x(p3-p1)  Z2x(p3-p2) 

Zo  Z 1  22 


Computation  of  the  position  vectors  of  the  various  links  gives 


'o' 

a2CiC2 

Po  =  Pi  = 

0 

P2  = 

a2siC2 

0 

a2s2 

ci(a2c2  +  <23023) 
si(<22C2  +  03C23) 
<22  S2  +  CI3S23 


P  3  = 


3.2  Jacobian  of  Typical  Manipulator  Structures  115 


while  computation  of  the  unit  vectors  of  revolute  joint  axes  gives 


From  (3.29)  it  is 


-si(a2c2  +  a3c23) 
ci(a2c2  +  a3c23) 
0 
0 
0 
1 


— Ci(a2s2  +  a3s23)  -a3cis23 
-Si  (a2s2  +  a3s23)  -a3sis23 
a2c2  +  a3c23  a3c23 

si  Si 

-Ci  -Ci 

0  0 


(3.37) 


Only  three  of  the  six  rows  of  the  Jacobian  (3.37)  are  linearly  independent. 
Having  3  DOFs  only,  it  is  worth  considering  the  upper  (3  x  3)  block  of  the 
Jacobian 

— si(a2c2  +  a3c23)  -Ci(a2s2  +  a3s23)  -a3Cis23 
Jp=  Ci(a2c2  +  a3c23)  —  Si(a2s2  +  a3s23)  — a3sis23  (3.38) 

0  a2c2  +  a3c23  a3c23 

that  describes  the  relationship  between  the  joint  velocities  and  the  end-effector 
linear  velocity.  This  structure  does  not  allow  an  arbitrary  angular  velocity 
to  be  obtained;  in  fact,  the  two  components  u>x  and  u>y  are  not  independent 

(SiUJy  —  CiLOx). 


3.2.3  Stanford  Manipulator 

In  this  case,  from  (3.30)  it  is 

z0x(p6-Pq)  zi  x  (pe  —  Pi)  22 
z0  z  1  0 

23  x  (p6  -  p3)  z 4  x  (p6  -  p4)  z5  x  (p6  -  p5) 
z  3  2  4  25 

Computation  of  the  position  vectors  of  the  various  links  gives 

0  CiS2C?3  —  SiG?2 

Po  =  Pi=  0  P3  =  P4=P5=  SiS2d3  +  Cid2 
0  c2d3 

ClS2d3  —  Sid2  +  (ci(c2C4S5  +  S2C5)  —  SiS4Ss)(i6 

Pq  —  Sis2d3  +  Cid2  +  (si(c2c4s5  +  s2c5)  +  Cis4s5)(i6  > 
c2d3  +  (  — S2C4S5  +  c2  05)1^6 
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while  computation  of  the  unit  vectors  of  joint  axes  gives 


'O' 

-si 

C1S2 

0 

Zl  = 

Cl 

II 

CO 

fc* 

II 

(M 

S1S2 

1 

0 

C2 

—  CiC2S4  —  S1C4 

Ci(c2C4S5  +  S2C5)  —  S1S4S5 

24  = 

—  SiC2S4  T  C1C4 

25  = 

Si(c2C4S5  +  S2Cs)  +  C1S4S5 

S2S4 

—  S2C4S5  +  C2  C5 

The  sought  Jacobian  can  be  obtained  by  developing  the  computations  as 
in  (3.29),  leading  to  expressing  end-effector  linear  and  angular  velocity  as 
a  function  of  joint  velocities. 


3.3  Kinematic  Singularities 

The  Jacobian  in  the  differential  kinematics  equation  of  a  manipulator  defines 
a  linear  mapping 

ve  =  J  {q)q  (3.39) 

between  the  vector  q  of  joint  velocities  and  the  vector  ve  =  [pj  }T  of  end- 
effector  velocity.  The  Jacobian  is,  in  general,  a  function  of  the  configuration 
q;  those  configurations  at  which  J  is  rank-deficient  are  termed  kinematic 
singularities.  To  find  the  singularities  of  a  manipulator  is  of  great  interest  for 
the  following  reasons: 

a)  Singularities  represent  configurations  at  which  mobility  of  the  structure 
is  reduced,  i.e.,  it  is  not  possible  to  impose  an  arbitrary  motion  to  the 
end-effector. 

b)  When  the  structure  is  at  a  singularity,  infinite  solutions  to  the  inverse 
kinematics  problem  may  exist. 

c)  In  the  neighbourhood  of  a  singularity,  small  velocities  in  the  operational 
space  may  cause  large  velocities  in  the  joint  space. 

Singularities  can  be  classified  into: 

•  Boundary  singularities  that  occur  when  the  manipulator  is  either  out¬ 
stretched  or  retracted.  It  may  be  understood  that  these  singularities  do 
not  represent  a  true  drawback,  since  they  can  be  avoided  on  condition  that 
the  manipulator  is  not  driven  to  the  boundaries  of  its  reachable  workspace. 

•  Internal  singularities  that  occur  inside  the  reachable  workspace  and  are 
generally  caused  by  the  alignment  of  two  or  more  axes  of  motion,  or  else  by 
the  attainment  of  particular  end-effector  configurations.  Unlike  the  above, 
these  singularities  constitute  a  serious  problem,  as  they  can  be  encountered 
anywhere  in  the  reachable  workspace  for  a  planned  path  in  the  operational 
space. 
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Fig.  3.3.  Two-link  planar  arm  at  a  boundary  singularity 


Example  3.2 

To  illustrate  the  behaviour  of  a  manipulator  at  a  singularity,  consider  a  two-link 
planar  arm.  In  this  case,  it  is  worth  considering  only  the  components  px  and  py  of 
the  linear  velocity  in  the  plane.  Thus,  the  Jacobian  is  the  (2  x  2)  matrix 


J  = 


—  CliSi  —  CL2S12 
aici  +  C12C12 


—C12S12 

U2C12 


To  analyze  matrix  rank,  consider  its  determinant  given  by 


det(J)  =  aia,2S2- 


(3.40) 


(3.41) 


For  ai,ci2  ^  0,  it  is  easy  to  find  that  the  determinant  in  (3.41)  vanishes  whenever 


$2  =  0  $2  =  7T, 


$1  being  irrelevant  for  the  determination  of  singular  configurations.  These  occur 
when  the  arm  tip  is  located  either  on  the  outer  ($2  =  0)  or  on  the  inner  ($2  =  n) 
boundary  of  the  reachable  workspace.  Figure  3.3  illustrates  the  arm  posture  for 
$2  =  0. 

By  analyzing  the  differential  motion  of  the  structure  in  such  configuration,  it 
can  be  observed  that  the  two  column  vectors  [ —(ai  +  02)^1  (ai  +  02)01  ]T  and 
[—  <Z2Si  a2Ci  ]T  of  the  Jacobian  become  parallel,  and  thus  the  Jacobian  rank  be¬ 
comes  one;  this  means  that  the  tip  velocity  components  are  not  independent  (see 
point  a)  above). 


3.3.1  Singularity  Decoupling 

Computation  of  internal  singularities  via  the  Jacobian  determinant  may  be 
tedious  and  of  no  easy  solution  for  complex  structures.  For  manipulators  hav¬ 
ing  a  spherical  wrist,  by  analogy  with  what  has  already  been  seen  for  inverse 
kinematics,  it  is  possible  to  split  the  problem  of  singularity  computation  into 
two  separate  problems: 
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-A  =  0 


Fig.  3.4.  Spherical  wrist  at  a  singularity 


•  computation  of  arm  singularities  resulting  from  the  motion  of  the  first  3 
or  more  links, 

•  computation  of  wrist  singularities  resulting  from  the  motion  of  the  wrist 
joints. 

For  the  sake  of  simplicity,  consider  the  case  n  =  6;  the  Jacobian  can  be 
partitioned  into  (3  x  3)  blocks  as  follows: 


J 11  J 12 
J 21  J 22 


(3.42) 


where,  since  the  outer  3  joints  are  all  revolute,  the  expressions  of  the  two  right 
blocks  are  respectively 

J12  =  [>3  x  (pe  -p3)  z4x{pe-p4)  z5  x  (pe  —  p5)  ] 

J  22  =  \z3  ZA  25]-  (3.43) 

As  singularities  are  typical  of  the  mechanical  structure  and  do  not  depend  on 
the  frames  chosen  to  describe  kinematics,  it  is  convenient  to  choose  the  origin 
of  the  end-effector  frame  at  the  intersection  of  the  wrist  axes  (see  Fig.  2.32). 
The  choice  p  =  pw  leads  to 


Ji2=[0  0  0], 

since  all  vectors  pw  —  pt  are  parallel  to  the  unit  vectors  z*,  for  i  =  3,4,  5,  no 
matter  how  Frames  3, 4, 5  are  chosen  according  to  DH  convention.  In  view  of 
this  choice,  the  overall  Jacobian  becomes  a  block  lower-triangular  matrix.  In 
this  case,  computation  of  the  determinant  is  greatly  simplified,  as  this  is  given 
by  the  product  of  the  determinants  of  the  two  blocks  on  the  diagonal,  i.e. , 

det(J)  =  det(Jn)det(J22).  (3.44) 

In  turn,  a  true  singularity  decoupling  has  been  achieved;  the  condition 


det(Jn)  =  0 
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leads  to  determining  the  arm  singularities ,  while  the  condition 

det(J22)  —  0 

leads  to  determining  the  wrist  singularities . 

Notice,  however,  that  this  form  of  Jacobian  does  not  provide  the  relation¬ 
ship  between  the  joint  velocities  and  the  end-effector  velocity,  but  it  leads  to 
simplifying  singularity  computation.  Below  the  two  types  of  singularities  are 
analyzed  in  detail. 

3.3.2  Wrist  Singularities 

On  the  basis  of  the  above  singularity  decoupling,  wrist  singularities  can  be 
determined  by  inspecting  the  block  J22  in  (3.43).  It  can  be  recognized  that  the 
wrist  is  at  a  singular  configuration  whenever  the  unit  vectors  z3,  z 4,  z5  are 
linearly  dependent.  The  wrist  kinematic  structure  reveals  that  a  singularity 
occurs  when  z3  and  z3  are  aligned,  i.e.,  whenever 

$5  =  0  $5  =  7T. 

Taking  into  consideration  only  the  first  configuration  (Fig.  3.4),  the  loss  of 
mobility  is  caused  by  the  fact  that  rotations  of  equal  magnitude  about  opposite 
directions  on  $4  and  do  not  produce  any  end-effector  rotation.  Further,  the 
wrist  is  not  allowed  to  rotate  about  the  axis  orthogonal  to  z 4  and  z3,  (see 
point  a)  above).  This  singularity  is  naturally  described  in  the  joint  space  and 
can  be  encountered  anywhere  inside  the  manipulator  reachable  workspace;  as 
a  consequence,  special  care  is  to  be  taken  in  programming  an  end-effector 
motion. 

3.3.3  Arm  Singularities 

Arm  singularities  are  characteristic  of  a  specific  manipulator  structure;  to 
illustrate  their  determination,  consider  the  anthropomorphic  arm  (Fig.  2.23), 
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whose  Jacobian  for  the  linear  velocity  part  is  given  by  (3.38).  Its  determinant 
is 

det(  J  P)  =  -a2a3s3(a2c2  +  a3c23). 

Like  in  the  case  of  the  planar  arm  of  Example  3.2,  the  determinant  does  not 
depend  on  the  first  joint  variable. 

For  a2,  a3  ^  0,  the  determinant  vanishes  if  S3  =  0  and/or  (a2c2  +  a3c23)  = 
0.  The  first  situation  occurs  whenever 

$3  =  0  $3  =  n 

meaning  that  the  elbow  is  outstretched  (Fig.  3.5)  or  retracted,  and  is  termed 
elbow  singularity.  Notice  that  this  type  of  singularity  is  conceptually  equiva¬ 
lent  to  the  singularity  found  for  the  two-link  planar  arm. 

By  recalling  the  direct  kinematics  equation  in  (2.66),  it  can  be  observed 
that  the  second  situation  occurs  when  the  wrist  point  lies  on  axis  zo  (Fig.  3.6); 
it  is  thus  characterized  by 

Px=Py  =  0 

and  is  termed  shoulder  singularity. 

Notice  that  the  whole  axis  Zo  describes  a  continuum  of  singular  configu¬ 
rations;  a  rotation  of  $1  does  not  cause  any  translation  of  the  wrist  position 
(the  first  column  of  J  p  is  always  null  at  a  shoulder  singularity),  and  then  the 
inverse  kinematics  equation  admits  infinite  solutions;  moreover,  motions  start¬ 
ing  from  the  singular  configuration  that  take  the  wrist  along  the  z3  direction 
are  not  allowed  (see  point  b)  above). 

If  a  spherical  wrist  is  connected  to  an  anthropomorphic  arm  (Fig.  2.26), 
the  arm  direct  kinematics  is  different.  In  this  case  the  Jacobian  to  consider 
represents  the  block  J n  of  the  Jacobian  in  (3.42)  with  p  =  pw.  Analyzing  its 
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determinant  leads  to  finding  the  same  singular  configurations,  which  are  rela¬ 
tive  to  different  values  of  the  third  joint  variables,  though  —  compare  (2.66) 
and  (2.70). 

Finally,  it  is  important  to  remark  that,  unlike  the  wrist  singularities,  the 
arm  singularities  are  well  identified  in  the  operational  space,  and  thus  they 
can  be  suitably  avoided  in  the  end-effector  trajectory  planning  stage. 


3.4  Analysis  of  Redundancy 

The  concept  of  kinematic  redundancy  has  been  introduced  in  Sect.  2.10.2; 
redundancy  is  related  to  the  number  n  of  DOFs  of  the  structure,  the  number  m 
of  operational  space  variables,  and  the  number  r  of  operational  space  variables 
necessary  to  specify  a  given  task. 

In  order  to  perform  a  systematic  analysis  of  redundancy,  it  is  worth  con¬ 
sidering  differential  kinematics  in  lieu  of  direct  kinematics  (2.82).  To  this  end, 
(3.39)  is  to  be  interpreted  as  the  differential  kinematics  mapping  relating  the 
n  components  of  the  joint  velocity  vector  to  the  r  <  m  components  of  the  ve¬ 
locity  vector  ve  of  concern  for  the  specific  task.  To  clarify  this  point,  consider 
the  case  of  a  3-link  planar  arm;  that  is  not  intrinsically  redundant  (n  =  m  =  3) 
and  its  Jacobian  (3.35)  has  3  null  rows  accordingly.  If  the  task  does  not  spec¬ 
ify  u>z  (r  =  2),  the  arm  becomes  functionally  redundant  and  the  Jacobian  to 
consider  for  redundancy  analysis  is  the  one  in  (3.36). 

A  different  case  is  that  of  the  anthropomorphic  arm  for  which  only  posi¬ 
tion  variables  are  of  concern  (n  =  m  =  3).  The  relevant  Jacobian  is  the  one 
in  (3.38).  The  arm  is  neither  intrinsically  redundant  nor  can  become  function¬ 
ally  redundant  if  it  is  assigned  a  planar  task;  in  that  case,  indeed,  the  task 
would  set  constraints  on  the  3  components  of  end-effector  linear  velocity. 

Therefore,  the  differential  kinematics  equation  to  consider  can  be  formally 
written  as  in  (3.39),  i.e. , 

ve  =  J(q)q ,  (3.45) 

where  now  ve  is  meant  to  be  the  (r  x  1)  vector  of  end-effector  velocity  of 
concern  for  the  specific  task  and  J  is  the  corresponding  (r  x  n )  Jacobian 
matrix  that  can  be  extracted  from  the  geometric  Jacobian;  q  is  the  (n  x  1) 
vector  of  joint  velocities.  If  r  <  n,  the  manipulator  is  kinematically  redundant 
and  there  exist  (n  —  r)  redundant  DOFs. 

The  Jacobian  describes  the  linear  mapping  from  the  joint  velocity  space  to 
the  end-effector  velocity  space.  In  general,  it  is  a  function  of  the  configuration. 
In  the  context  of  differential  kinematics,  however,  the  Jacobian  has  to  be 
regarded  as  a  constant  matrix,  since  the  instantaneous  velocity  mapping  is 
of  interest  for  a  given  posture.  The  mapping  is  schematically  illustrated  in 
Fig.  3.7  with  a  typical  notation  from  set  theory. 
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Fig.  3.7.  Mapping  between  the  joint  velocity  space  and  the  end-effector  velocity 
space 


The  differential  kinematics  equation  in  (3.45)  can  be  characterized  in  terms 
of  the  range  and  null  spaces  of  the  mapping;2  specifically,  one  has  that: 

•  The  range  space  of  J  is  the  subspace  7 Z(J)  in  ]Rr  of  the  end-effector  veloc¬ 
ities  that  can  be  generated  by  the  joint  velocities,  in  the  given  manipulator 
posture. 

•  The  null  space  of  J  is  the  subspace  A f(J)  in  IRrl  of  joint  velocities  that  do 
not  produce  any  end-effector  velocity,  in  the  given  manipulator  posture. 

If  the  Jacobian  has  full  rank ,  one  has 

dim(7^(J))  =  r  dim(A/"(J))  =  n  —  r 

and  the  range  of  J  spans  the  entire  space  IFF'.  Instead,  if  the  Jacobian  degen¬ 
erates  at  a  singularity ,  the  dimension  of  the  range  space  decreases  while  the 
dimension  of  the  null  space  increases,  since  the  following  relation  holds: 

dim(7£(«7))  +  dim(A f(J))  =  n 

independently  of  the  rank  of  the  matrix  J . 

The  existence  of  a  subspace  Af(J)  ^  0  for  a  redundant  manipulator  allows 
determination  of  systematic  techniques  for  handling  redundant  DOFs.  To  this 
end,  if  q*  denotes  a  solution  to  (3.45)  and  P  is  an  (n  x  n)  matrix  so  that 

n{P)=U{j ), 


the  joint  velocity  vector 

q=q*+Pq0 ,  (3-46) 

with  arbitrary  qQ,  is  also  a  solution  to  (3.45).  In  fact,  premultiplying  both 
sides  of  (3.46)  by  J  yields 

Jq  =  J  q*  +  JPqQ  =  Jq*  =  Ve 

See  Sect.  A. 4  for  the  linear  mappings. 
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since  JPQo  =  0  for  any  q0.  This  result  is  of  fundamental  importance  for 
redundancy  resolution;  a  solution  of  the  kind  (3.46)  points  out  the  possibility 
of  choosing  the  vector  of  arbitrary  joint  velocities  q0  so  as  to  exploit  advanta¬ 
geously  the  redundant  DOFs.  In  fact,  the  effect  of  qQ  is  to  generate  internal 
motions  of  the  structure  that  do  not  change  the  end-effector  position  and  ori¬ 
entation  but  may  allow,  for  instance,  manipulator  reconfiguration  into  more 
dexterous  postures  for  execution  of  a  given  task. 


3.5  Inverse  Differential  Kinematics 

In  Sect.  2.12  it  was  shown  how  the  inverse  kinematics  problem  admits  closed- 
form  solutions  only  for  manipulators  having  a  simple  kinematic  structure. 
Problems  arise  whenever  the  end-effector  attains  a  particular  position  and/or 
orientation  in  the  operational  space,  or  the  structure  is  complex  and  it  is  not 
possible  to  relate  the  end-effector  pose  to  different  sets  of  joint  variables,  or 
else  the  manipulator  is  redundant.  These  limitations  are  caused  by  the  highly 
nonlinear  relationship  between  joint  space  variables  and  operational  space 
variables. 

On  the  other  hand,  the  differential  kinematics  equation  represents  a  linear 
mapping  between  the  joint  velocity  space  and  the  operational  velocity  space, 
although  it  varies  with  the  current  configuration.  This  fact  suggests  the  pos¬ 
sibility  to  utilize  the  differential  kinematics  equation  to  tackle  the  inverse 
kinematics  problem. 

Suppose  that  a  motion  trajectory  is  assigned  to  the  end-effector  in  terms 
of  ve  and  the  initial  conditions  on  position  and  orientation.  The  aim  is  to 
determine  a  feasible  joint  trajectory  (q(t),q(t))  that  reproduces  the  given 
trajectory. 

By  considering  (3.45)  with  n  =  r,  the  joint  velocities  can  be  obtained  via 
simple  inversion  of  the  Jacobian  matrix 

q  =  J~1(q)ve.  (3.47) 

If  the  initial  manipulator  posture  q(0)  is  known,  joint  positions  can  be  com¬ 
puted  by  integrating  velocities  over  time,  i.e., 

q(t)  =  I  <?(<?)*  +  <?( 0). 

Jo 

The  integration  can  be  performed  in  discrete  time  by  resorting  to  numerical 
techniques.  The  simplest  technique  is  based  on  the  Euler  integration  method; 
given  an  integration  interval  At,  if  the  joint  positions  and  velocities  at  time 
tk  are  known,  the  joint  positions  at  time  tk+i  =  tk  +  At  can  be  computed  as 


q(tk+ i)  =  q(tk)  +  q(tk)At. 


(3.48) 
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This  technique  for  inverting  kinematics  is  independent  of  the  solvability 
of  the  kinematic  structure.  Nonetheless,  it  is  necessary  that  the  Jacobian  be 
square  and  of  full  rank ;  this  demands  further  insight  into  the  cases  of  redun¬ 
dant  manipulators  and  kinematic  singularity  occurrence. 

3.5.1  Redundant  Manipulators 

When  the  manipulator  is  redundant  (r  <  n),  the  Jacobian  matrix  has  more 
columns  than  rows  and  infinite  solutions  exist  to  (3.45).  A  viable  solution 
method  is  to  formulate  the  problem  as  a  constrained  linear  optimization  prob¬ 
lem. 

In  detail,  once  the  end-effector  velocity  ve  and  Jacobian  J  are  given  (for 
a  given  configuration  q),  it  is  desired  to  find  the  solutions  q  that  satisfy  the 
linear  equation  in  (3.45)  and  minimize  the  quadratic  cost  functional  of  joint 
velocities3 

g(q)  =  \qlWq 

where  W  is  a  suitable  (n  x  n)  symmetric  positive  definite  weighting  matrix. 

This  problem  can  be  solved  with  the  method  of  Lagrange  multipliers.  Con¬ 
sider  the  modified  cost  functional 

d(q,  A)  =  \qrWq  +  \T(ve  -  Jq ), 

where  A  is  an  (r  x  1)  vector  of  unknown  multipliers  that  allows  the  incorpo¬ 
ration  of  the  constraint  (3.45)  in  the  functional  to  minimize.  The  requested 
solution  has  to  satisfy  the  necessary  conditions: 


From  the  first  one,  it  is  Wq  -  JT  A  =  0  and  thus 

q  =  W~1JT\  (3.49) 

where  the  inverse  of  W  exists.  Notice  that  the  solution  (3.49)  is  a  minimum, 
since  d2g/dq2  =  W  is  positive  definite.  From  the  second  condition  above,  the 
constraint 

ve  =  Jq 

is  recovered.  Combining  the  two  conditions  gives 

ve  =  JWXJT  A; 

under  the  assumption  that  J  has  full  rank,  JW  2JT  is  an  (r  x  r)  square 
matrix  of  rank  r  and  thus  can  be  inverted.  Solving  for  A  yields 

A  =  {JW~1JT)~1ve 

3  Quadratic  forms  and  the  relative  operations  are  recalled  in  Sect.  A. 6. 
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which,  substituted  into  (3.49),  gives  the  sought  optimal  solution 

q  =  W^1  JT  (JW1  JT)~1ve.  (3.50) 

Premultiplying  both  sides  of  (3.50)  by  J,  it  is  easy  to  verify  that  this  solution 
satisfies  the  differential  kinematics  equation  in  (3.45). 

A  particular  case  occurs  when  the  weighting  matrix  W  is  the  identity 
matrix  I  and  the  solution  simplifies  into 

q  =  JfUe;  (3.51) 

the  matrix 

jt  =  JT(JJT)-!  (3.52) 

is  the  right  pseudo-inverse  of  J.4  The  obtained  solution  locally  minimizes  the 
norm  of  joint  velocities. 

It  was  pointed  out  above  that  if  q*  is  a  solution  to  (3.45),  q*  +  PqQ  is  also  a 
solution,  where  q0  is  a  vector  of  arbitrary  joint  velocities  and  P  is  a  projector 
in  the  null  space  of  J .  Therefore,  in  view  of  the  presence  of  redundant  DOFs, 
the  solution  (3.51)  can  be  modified  by  the  introduction  of  another  term  of 
the  kind  PQ0  In  particular,  q0  can  be  specified  so  as  to  satisfy  an  additional 
constraint  to  the  problem. 

In  that  case,  it  is  necessary  to  consider  a  new  cost  functional  in  the  form 

9'{q)  =  \(<i-  <?0)T(<?-  <z0); 

this  choice  is  aimed  at  minimizing  the  norm  of  vector  q  —  q0:  in  other  words, 
solutions  are  sought  which  satisfy  the  constraint  (3.45)  and  are  as  close  as  pos¬ 
sible  to  qQ.  In  this  way,  the  objective  specified  through  q0  becomes  unavoid¬ 
ably  a  secondary  objective  to  satisfy  with  respect  to  the  primary  objective 
specified  by  the  constraint  (3.45). 

Proceeding  in  a  way  similar  to  the  above  yields 

9'(q,X)  =  ^(q-q0)T(q-q0)  +  XT(ve-Jq); 
from  the  first  necessary  condition  it  is 

q  =  JT\  +  qn  (3.53) 

which,  substituted  into  (3.45),  gives 

A  =  (JJT)~1(ve-Jq0). 

Finally,  substituting  A  back  in  (3.53)  gives 

q  =  J^ve  +  (In  -  J^J)q0.  (3.54) 

See  Sect.  A. 7  for  the  definition  of  the  pseudo-inverse  of  a  matrix. 
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As  can  be  easily  recognized,  the  obtained  solution  is  composed  of  two  terms. 
The  first  is  relative  to  minimum  norm  joint  velocities.  The  second,  termed 
homogeneous  solution,  attempts  to  satisfy  the  additional  constraint  to  specify 
via  q0;5  the  matrix  (7  —  J)  is  one  of  those  matrices  P  introduced  in  (3.46) 
which  allows  the  projection  of  the  vector  q0  in  the  null  space  of  J,  so  as 
not  to  violate  the  constraint  (3.45).  A  direct  consequence  is  that,  in  the  case 
ve  =  0,  is  is  possible  to  generate  internal  motions  described  by  (7  —  J)q0 
that  reconfigure  the  manipulator  structure  without  changing  the  end-effector 
position  and  orientation. 

Finally,  it  is  worth  discussing  the  way  to  specify  the  vector  q0  for  a  con¬ 
venient  utilization  of  redundant  DOFs.  A  typical  choice  is 


Qo  =  ko 


(dw(g)^ 


T 


(3.55) 


where  fc0  >  0  and  w(q)  is  a  (secondary)  objective  function  of  the  joint  vari¬ 
ables.  Since  the  solution  moves  along  the  direction  of  the  gradient  of  the  ob¬ 
jective  function,  it  attempts  to  maximize  it  locally  compatible  to  the  primary 
objective  (kinematic  constraint).  Typical  objective  functions  are: 

•  The  manipulability  measure ,  defined  as 


w{q)  =  *\J  det(  J  (q)JT(q))  (3.56) 

which  vanishes  at  a  singular  configuration;  thus,  by  maximizing  this  mea¬ 
sure,  redundancy  is  exploited  to  move  away  from  singularities.6 
•  The  distance  from  mechanical  joint  limits,  defined  as 


w{q) 


n  ,  \  2 

1  /  Qi  ~  Qi  I 

2 n  ^  \  qtM  ~  qim  J 


(3.57) 


where  qiM  (qim)  denotes  the  maximum  (minimum)  joint  limit  and  qi  the 
middle  value  of  the  joint  range;  thus,  by  maximizing  this  distance,  redun¬ 
dancy  is  exploited  to  keep  the  joint  variables  as  close  as  possible  to  the 
centre  of  their  ranges. 

•  The  distance  from  an  obstacle,  defined  as 


w(q)  =  min  ||p(q)  —  o||  (3.58) 

where  o  is  the  position  vector  of  a  suitable  point  on  the  obstacle  (its 
centre,  for  instance,  if  the  obstacle  is  modelled  as  a  sphere)  and  p  is  the 

5  It  should  be  recalled  that  the  additional  constraint  has  secondary  priority  with 
respect  to  the  primary  kinematic  constraint. 

6  The  manipulability  measure  is  given  by  the  product  of  the  singular  values  of  the 
Jacobian  (see  Problem  3.8). 
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position  vector  of  a  generic  point  along  the  structure;  thus,  by  maximizing 
this  distance,  redundancy  is  exploited  to  avoid  collision  of  the  manipulator 
with  an  obstacle  (see  also  Problem  3. 9). 7 


3.5.2  Kinematic  Singularities 

Both  solutions  (3.47)  and  (3.51)  can  be  computed  only  when  the  Jacobian 
has  full  rank.  Hence,  they  become  meaningless  when  the  manipulator  is  at  a 
singular  configuration;  in  such  a  case,  the  system  ve  =  Jq  contains  linearly 
dependent  equations. 

It  is  possible  to  find  a  solution  q  by  extracting  all  the  linearly  independent 
equations  only  if  ve  £  TZ(J).  The  occurrence  of  this  situation  means  that  the 
assigned  path  is  physically  executable  by  the  manipulator,  even  though  it  is 
at  a  singular  configuration.  If  instead  ve  ^  7 Z(J),  the  system  of  equations  has 
no  solution;  this  means  that  the  operational  space  path  cannot  be  executed 
by  the  manipulator  at  the  given  posture. 

It  is  important  to  underline  that  the  inversion  of  the  Jacobian  can  represent 
a  serious  inconvenience  not  only  at  a  singularity  but  also  in  the  neighbourhood 
of  a  singularity.  For  instance,  for  the  Jacobian  inverse  it  is  well  known  that  its 
computation  requires  the  computation  of  the  determinant;  in  the  neighbour¬ 
hood  of  a  singularity,  the  determinant  takes  on  a  relatively  small  value  which 
can  cause  large  joint  velocities  (see  point  c)  in  Sect.  3.3).  Consider  again  the 
above  example  of  the  shoulder  singularity  for  the  anthropomorphic  arm.  If  a 
path  is  assigned  to  the  end-effector  which  passes  nearby  the  base  rotation  axis 
(geometric  locus  of  singular  configurations),  the  base  joint  is  forced  to  make 
a  rotation  of  about  tt  in  a  relatively  short  time  to  allow  the  end-effector  to 
keep  tracking  the  imposed  trajectory. 

A  more  rigorous  analysis  of  the  solution  features  in  the  neighbourhood  of 
singular  configurations  can  be  developed  by  resorting  to  the  singular  value 
decomposition  (SVD)  of  matrix  J.8 

An  alternative  solution  overcoming  the  problem  of  inverting  differential 
kinematics  in  the  neighbourhood  of  a  singularity  is  provided  by  the  so-called 
damped  least-squares  (DLS)  inverse 

J*  =  JT(JJT  +  fc2/)-1  (3.59) 

where  k  is  a  damping  factor  that  renders  the  inversion  better  conditioned 
from  a  numerical  viewpoint.  It  can  be  shown  that  such  a  solution  can  be 


1  If  an  obstacle  occurs  along  the  end-effector  path,  it  is  opportune  to  invert  the 
order  of  priority  between  the  kinematic  constraint  and  the  additional  constraint; 
in  this  way  the  obstacle  may  be  avoided,  but  one  gives  up  tracking  the  desired 
path. 

8  See  Sect.  A. 8. 
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obtained  by  reformulating  the  problem  in  terms  of  the  minimization  of  the 
cost  functional 

g"(Q)  =  -  Jq)T{ve  ~  Jq)  +  ^ k2qTq , 

where  the  introduction  of  the  first  term  allows  a  finite  inversion  error  to  be 
tolerated,  with  the  advantage  of  norm-bounded  velocities.  The  factor  k  es¬ 
tablishes  the  relative  weight  between  the  two  objectives,  and  there  exist  tech¬ 
niques  for  selecting  optimal  values  for  the  damping  factor  (see  Problem  3.10). 


3.6  Analytical  Jacobian 


The  above  sections  have  shown  the  way  to  compute  the  end-effector  velocity 
in  terms  of  the  velocity  of  the  end-effector  frame.  The  Jacobian  is  computed 
according  to  a  geometric  technique  in  which  the  contributions  of  each  joint 
velocity  to  the  components  of  end-effector  linear  and  angular  velocity  are 
determined. 

If  the  end-effector  pose  is  specified  in  terms  of  a  minimal  number  of  pa¬ 
rameters  in  the  operational  space  as  in  (2.80),  it  is  natural  to  ask  whether 
it  is  possible  to  compute  the  Jacobian  via  differentiation  of  the  direct  kine¬ 
matics  function  with  respect  to  the  joint  variables.  To  this  end,  an  analytical 
technique  is  presented  below  to  compute  the  Jacobian,  and  the  existing  rela¬ 
tionship  between  the  two  Jacobians  is  found. 

The  translational  velocity  of  the  end-effector  frame  can  be  expressed  as 
the  time  derivative  of  vector  pe ,  representing  the  origin  of  the  end-effector 
frame  with  respect  to  the  base  frame,  i.e., 

pe=9^q  =  JP(q)q.  (3.60) 

For  what  concerns  the  rotational  velocity  of  the  end-effector  frame,  the 
minimal  representation  of  orientation  in  terms  of  three  variables  <pe  can  be 
considered.  Its  time  derivative  cf)e  in  general  differs  from  the  angular  velocity 
vector  defined  above.  In  any  case,  once  the  function  4>e(q)  is  known,  it  is 
formally  correct  to  consider  the  Jacobian  obtained  as 

4>e  =  d$^q  =  J<t(q)q-  (3.61) 


Computing  the  Jacobian  J ^(q)  as  d4>e/dq  is  not  straightforward,  since  the 
function  <j>e(q)  is  not  usually  available  in  direct  form,  but  requires  computation 
of  the  elements  of  the  relative  rotation  matrix. 

Upon  these  premises,  the  differential  kinematics  equation  can  be  obtained 
as  the  time  derivative  of  the  direct  kinematics  equation  in  (2.82),  i.e., 


xe 


Pe' 

'Jp(q)' 

A. 

. J<t>(q ). 

q  =  JA{q)q 


(3.62) 
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Fig.  3.8.  Rotational  velocities  of  Euler  angles  ZYZ  in  current  frame 


Fig.  3.9.  Composition  of  elementary  rotational  velocities  for  computing  angular 
velocity 


where  the  analytical  Jacobian 

J  ,ta)  =  ^  (3.63) 

is  different  from  the  geometric  Jacobian  J ,  since  the  end-effector  angular 
velocity  tue  with  respect  to  the  base  frame  is  not  given  by  <pe. 

It  is  possible  to  find  the  relationship  between  the  angular  velocity  u>e  and 
the  rotational  velocity  <f)e  for  a  given  set  of  orientation  angles.  For  instance, 
consider  the  Euler  angles  ZYZ  defined  in  Sect.  2.4.1;  in  Fig.  3.8,  the  vectors 
corresponding  to  the  rotational  velocities  dp,  $,  if)  have  been  represented  with 
reference  to  the  current  frame.  Figure  3.9  illustrates  how  to  compute  the 
contributions  of  each  rotational  velocity  to  the  components  of  angular  velocity 
about  the  axes  of  the  reference  frame: 

•  as  a  result  of  ip:  [tox  uy  cuz]T  =  ^>[0  0  1]T 

•  as  a  result  of  i?:  [u>x  uiy  u)z]T  =  sv  cv  0]T 
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as  a  result  of  ip:  [ivx  tvy  ivz  ]T  =  ip  [cvs$  c,jp 


and  then  the  equation  relating  the  angular  velocity  tve  to  the  time  derivative 
of  the  Euler  angles  cpe  is9 

=  T(4>e)4>e,  (3.64) 


where,  in  this  case, 


Sip 

e^pSifr 

SipStf 

0 

c# 

The  determinant  of  matrix  T  is  —  s&,  which  implies  that  the  relationship 
cannot  be  inverted  for  it)  =  0,7r.  This  means  that,  even  though  all  rotational 
velocities  of  the  end-effector  frame  can  be  expressed  by  means  of  a  suitable 
angular  velocity  vector  u>e,  there  exist  angular  velocities  which  cannot  be 
expressed  by  means  of  cpe  when  the  orientation  of  the  end-effector  frame  causes 
s$  =  O.10  In  fact,  in  this  situation,  the  angular  velocities  that  can  be  described 
by  4>e  should  have  linearly  dependent  components  in  the  directions  orthogonal 
to  axis  z  [tv2  +  tv2  =  it)2).  An  orientation  for  which  the  determinant  of  the 
transformation  matrix  vanishes  is  termed  representation  singularity  of  <pe. 

From  a  physical  viewpoint,  the  meaning  of  ive  is  more  intuitive  than  that 
of  4>e.  The  three  components  of  tve  represent  the  components  of  angular  veloc¬ 
ity  with  respect  to  the  base  frame.  Instead,  the  three  elements  of  (pe  represent 
nonorthogonal  components  of  angular  velocity  defined  with  respect  to  the 
axes  of  a  frame  that  varies  as  the  end-effector  orientation  varies.  On  the  other 
hand,  while  the  integral  of  4>e  over  time  gives  <pe,  the  integral  of  tve  does  not 
admit  a  clear  physical  interpretation,  as  can  be  seen  in  the  following  example. 


Example  3.3 

Consider  an  object  whose  orientation  with  respect  to  a  reference  frame  is  known  at 
time  t  =  0.  Assign  the  following  time  profiles  to  tv: 

•  tv  =  [n/2  0  0]T  0<t<l  w  =  [0  n/2  0]T  1  <  t  <  2, 

•  w  =  [0  n/2  Of  0<t<l  iv  =  [n/2  0  0]T  1  <  t  <  2. 

The  integral  of  tv  gives  the  same  result  in  the  two  cases 

ivdt  =  [n/2  n/2  0]T 

but  the  final  object  orientation  corresponding  to  the  second  timing  law  is  clearly 
different  from  the  one  obtained  with  the  first  timing  law  (Fig.  3.10). 


9  This  relation  can  also  be  obtained  from  the  rotation  matrix  associated  with  the 
three  angles  (see  Problem  3.11). 

10  In  Sect.  2.4.1,  it  was  shown  that  for  this  orientation  the  inverse  solution  of  the 
Euler  angles  degenerates. 
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Fig.  3.10.  Nonuniqueness  of  orientation  computed  as  the  integral  of  angular  velocity 


Once  the  transformation  T  between  u>e  and  4>e  is  given,  the  analytical 
Jacobian  can  be  related  to  the  geometric  Jacobian  as 


ve 


I  O 

O  T(0e) 


TA(4>e)xe 


(3.65) 


which,  in  view  of  (3.4),  (3.62),  yields 


J  =  ta(4>)j  a. 


(3.66) 


This  relationship  shows  that  J  and  J A,  in  general,  differ.  Regarding  the  use 
of  either  one  or  the  other  in  all  those  problems  where  the  influence  of  the 
Jacobian  matters,  it  is  anticipated  that  the  geometric  Jacobian  will  be  adopted 
whenever  it  is  necessary  to  refer  to  quantities  of  clear  physical  meaning,  while 
the  analytical  Jacobian  will  be  adopted  whenever  it  is  necessary  to  refer  to 
differential  quantities  of  variables  defined  in  the  operational  space. 

For  certain  manipulator  geometries,  it  is  possible  to  establish  a  substantial 
equivalence  between  J  and  J A.  In  fact,  when  the  DOFs  cause  rotations  of 
the  end-effector  all  about  the  same  fixed  axis  in  space,  the  two  Jacobians 
are  essentially  the  same.  This  is  the  case  of  the  above  three-link  planar  arm. 
Its  geometric  Jacobian  (3.35)  reveals  that  only  rotations  about  axis  zq  are 
permitted.  The  (3  x  3)  analytical  Jacobian  that  can  be  derived  by  considering 
the  end-effector  position  components  in  the  plane  of  the  structure  and  defining 
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the  end-effector  orientation  as  <f>  =  $1  +  t?2  +  $3  coincides  with  the  matrix 
that  is  obtained  by  eliminating  the  three  null  rows  of  the  geometric  Jacobian. 


3.7  Inverse  Kinematics  Algorithms 

In  Sect.  3.5  it  was  shown  how  to  invert  kinematics  by  using  the  differential 
kinematics  equation.  In  the  numerical  implementation  of  (3.48),  computation 
of  joint  velocities  is  obtained  by  using  the  inverse  of  the  Jacobian  evaluated 
with  the  joint  variables  at  the  previous  instant  of  time 

q(tk+ 1)  =  q(tk)  +  J~1(q(tk))ve(tk)At. 

It  follows  that  the  computed  joint  velocities  q  do  not  coincide  with  those 
satisfying  (3.47)  in  the  continuous  time.  Therefore,  reconstruction  of  joint 
variables  q  is  entrusted  to  a  numerical  integration  which  involves  drift  phe¬ 
nomena  of  the  solution;  as  a  consequence,  the  end-effector  pose  corresponding 
to  the  computed  joint  variables  differs  from  the  desired  one. 

This  inconvenience  can  be  overcome  by  resorting  to  a  solution  scheme  that 
accounts  for  the  operational  space  error  between  the  desired  and  the  actual 
end-effector  position  and  orientation.  Let 

e  =  xd  -  xe  (3.67) 

be  the  expression  of  such  error. 

Consider  the  time  derivative  of  (3.67),  i.e., 

e  =  xd  —  xe  (3.68) 

which,  according  to  differential  kinematics  (3.62),  can  be  written  as 

e  =  xd-JA(q)q.  (3.69) 

Notice  in  (3.69)  that  the  use  of  operational  space  quantities  has  naturally 

lead  to  using  the  analytical  Jacobian  in  lieu  of  the  geometric  Jacobian.  For 
this  equation  to  lead  to  an  inverse  kinematics  algorithm,  it  is  worth  relating 
the  computed  joint  velocity  vector  q  to  the  error  e  so  that  (3.69)  gives  a 
differential  equation  describing  error  evolution  over  time.  Nonetheless,  it  is 
necessary  to  choose  a  relationship  between  q  and  e  that  ensures  convergence 
of  the  error  to  zero. 

Having  formulated  inverse  kinematics  in  algorithmic  terms  implies  that 
the  joint  variables  q  corresponding  to  a  given  end-effector  pose  xd  are  ac¬ 
curately  computed  only  when  the  error  xd  —  k(q)  is  reduced  within  a  given 
threshold;  such  settling  time  depends  on  the  dynamic  characteristics  of  the 
error  differential  equation.  The  choice  of  q  as  a  function  of  e  permits  finding 
inverse  kinematics  algorithms  with  different  features. 
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Xd 


Fig.  3.11.  Inverse  kinematics  algorithm  with  Jacobian  inverse 


3.7.1  Jacobian  (Pseudo-) inverse 

On  the  assumption  that  matrix  J  a  is  square  and  nonsingular,  the  choice 

q=J~A\q){xd  +  Ke)  (3.70) 

leads  to  the  equivalent  linear  system 

e  +  Ke  =  0.  (3.71) 

If  If  is  a  positive  definite  (usually  diagonal)  matrix,  the  system  (3.71)  is 
asymptotically  stable.  The  error  tends  to  zero  along  the  trajectory  with  a 
convergence  rate  that  depends  on  the  eigenvalues  of  matrix  K'11  the  larger 
the  eigenvalues,  the  faster  the  convergence.  Since  the  scheme  is  practically 
implemented  as  a  discrete-time  system,  it  is  reasonable  to  predict  that  an 
upper  bound  exists  on  the  eigenvalues;  depending  on  the  sampling  time,  there 
will  be  a  limit  for  the  maximum  eigenvalue  of  K  under  which  asymptotic 
stability  of  the  error  system  is  guaranteed. 

The  block  scheme  corresponding  to  the  inverse  kinematics  algorithm 
in  (3.70)  is  illustrated  in  Fig.  3.11,  where  k(-)  indicates  the  direct  kinematics 
function  in  (2.82).  This  scheme  can  be  revisited  in  terms  of  the  usual  feedback 
control  schemes.  Specifically,  it  can  observed  that  the  nonlinear  block  fc(-)  is 
needed  to  compute  x  and  thus  the  tracking  error  e,  while  the  block 
has  been  introduced  to  compensate  for  J a(q)  and  making  the  system  linear. 
The  block  scheme  shows  the  presence  of  a  string  of  integrators  on  the  forward 
loop  and  then,  for  a  constant  reference  ( xd  =  0),  guarantees  a  null  steady- 
state  error.  Further,  the  feedforward  action  provided  by  Xd  for  a  time-varying 
reference  ensures  that  the  error  is  kept  to  zero  (in  the  case  e(0)  =  0)  along 
the  whole  trajectory,  independently  of  the  type  of  desired  reference  xd{f). 

Finally,  notice  that  (3.70),  for  xd  =  0,  corresponds  to  the  Newton  method 
for  solving  a  system  of  nonlinear  equations.  Given  a  constant  end-effector 
pose  Xd,  the  algorithm  can  be  keenly  applied  to  compute  one  of  the  admissible 


ii 


See  Sect.  A. 5. 
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Fig.  3.12.  Block  scheme  of  the  inverse  kinematics  algorithm  with  Jacobian  trans¬ 
pose 


solutions  to  the  inverse  kinematics  problem,  whenever  that  does  not  admit 
closed-form  solutions,  as  discussed  in  Sect.  2.12.  Such  a  method  is  also  useful 
in  practice  at  the  start-up  of  the  manipulator  for  a  given  task,  to  compute  the 
corresponding  joint  configuration. 

In  the  case  of  a  redundant  manipulator ,  solution  (3.70)  can  be  generalized 
into 

q  =  j\(xd  +  Ke)  +  (In  -  j\jA)qa,  (3.72) 

which  represents  the  algorithmic  version  of  solution  (3.54). 

The  structure  of  the  inverse  kinematics  algorithm  can  be  conceptually 
adopted  for  a  simple  robot  control  technique,  known  under  the  name  of  kine¬ 
matic  control.  As  will  be  seen  in  Chap.  7,  a  manipulator  is  actually  an  electro¬ 
mechanical  system  actuated  by  motor  torques,  while  in  Chaps.  8-10  dynamic 
control  techniques  will  be  presented  which  will  properly  account  for  the  non¬ 
linear  and  coupling  effects  of  the  dynamic  model. 

At  first  approximation,  however,  it  is  possible  to  consider  a  kinematic 
command  as  system  input,  typically  a  velocity.  This  is  possible  in  view  of 
the  presence  of  a  low-level  control  loop,  which  ‘ideally’  imposes  any  specified 
reference  velocity.  On  the  other  hand,  such  a  loop  already  exists  in  a  ‘closed’ 
control  unit,  where  the  user  can  also  intervene  with  kinematic  commands. 
In  other  words,  the  scheme  in  Fig.  3.11  can  implement  a  kinematic  control, 
provided  that  the  integrator  is  regarded  as  a  simplified  model  of  the  robot, 
thanks  to  the  presence  of  single  joint  local  servos,  which  ensure  a  more  or 
less  accurate  reproduction  of  the  velocity  commands.  Nevertheless,  it  is  worth 
underlining  that  such  a  kinematic  control  technique  yields  satisfactory  perfor¬ 
mance  only  when  one  does  not  require  too  fast  motions  or  rapid  accelerations. 
The  performance  of  the  independent  joint  control  will  be  analyzed  in  Sect.  8.3. 

3.7.2  Jacobian  Transpose 

A  computationally  simpler  algorithm  can  be  derived  by  finding  a  relationship 
between  q  and  e  that  ensures  error  convergence  to  zero,  without  requiring 
linearization  of  (3.69).  As  a  consequence,  the  error  dynamics  is  governed  by  a 
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nonlinear  differential  equation.  The  Lyapunov  direct  method  can  be  utilized 
to  determine  a  dependence  q(e)  that  ensures  asymptotic  stability  of  the  error 
system.  Choose  as  Lyapunov  function  candidate  the  positive  definite  quadratic 
form12 

V(e)  =  \eTKe,  (3.73) 

where  IT  is  a  symmetric  positive  definite  matrix.  This  function  is  so  that 
V(e)  >0  Ve  0,  V(0)  =  0. 

Differentiating  (3.73)  with  respect  to  time  and  accounting  for  (3.68)  gives 

V  =  eTKxd  -  eT Kxe.  (3.74) 

In  view  of  (3.62),  it  is 

V  =  eT Kxd  -  eTKJA(q)q.  (3.75) 

At  this  point,  the  choice  of  joint  velocities  as 

q  =  JTA(q)Ke  (3.76) 

leads  to 

V  =  eT Kxd  -  eTKJA{q)JTA{q)Ke.  (3.77) 

Consider  the  case  of  a  constant  reference  ( xd  =  0).  The  function  in  (3.77)  is 
negative  definite,  under  the  assumption  of  full  rank  for  J A(q).  The  condition 
V  <  0  with  V  >  0  implies  that  the  system  trajectories  uniformly  converge 
to  e  =  0,  i.e.,  the  system  is  asymptotically  stable.  When  A7(J^)  ^  0,  the 
function  in  (3.77)  is  only  negative  semi-definite,  since  V  =  0  for  e  /  0  with 
Ke  £  J\f(JA).  In  this  case,  the  algorithm  can  get  stuck  at  q  =  0  with  e/0. 
However,  the  example  that  follows  will  show  that  this  situation  occurs  only  if 
the  assigned  end-effector  position  is  not  actually  reachable  from  the  current 
configuration. 

The  resulting  block  scheme  is  illustrated  in  Fig.  3.12,  which  shows  the  no¬ 
table  feature  of  the  algorithm  to  require  computation  only  of  direct  kinematics 
functions  k(q),  j\(q). 

It  can  be  recognized  that  (3.76)  corresponds  to  the  gradient  method  for 
the  solution  of  a  system  on  nonlinear  equations.  As  in  the  case  of  the  Jaco¬ 
bian  inverse  solution,  for  a  given  constant  end-effector  pose  xd ,  the  Jacobian 
transpose  algorithm  can  be  keenly  employed  to  solve  the  inverse  kinemat¬ 
ics  problem,  or  more  simply  to  initialize  the  values  of  the  manipulator  joint 
variables. 

The  case  when  xd  is  a  time-varying  function  {xd  ^  0)  deserves  a  separate 
analysis.  In  order  to  obtain  V  <  0  also  in  this  case,  it  would  be  sufficient  to 
choose  a  q  that  depends  on  the  (pseudo-) inverse  of  the  Jacobian  as  in  (3.70), 


12 


See  Sect.  C.3  for  the  presentation  of  the  Lyapunov  direct  method. 
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Fig.  3.13.  Characterization  of  the  anthropomorphic  arm  at  a  shoulder  singularity 
for  the  admissible  solutions  of  the  Jacobian  transpose  algorithm 

recovering  the  asymptotic  stability  result  derived  above.13  For  the  inversion 
scheme  based  on  the  transpose,  the  first  term  on  the  right-hand  side  of  (3.77) 
is  not  cancelled  any  more  and  nothing  can  be  said  about  its  sign.  This  im¬ 
plies  that  asymptotic  stability  along  the  trajectory  cannot  be  achieved.  The 
tracking  error  e(t)  is,  anyhow,  norm-bounded;  the  larger  the  norm  of  K ,  the 
smaller  the  norm  of  e.14  In  practice,  since  the  inversion  scheme  is  to  be  im¬ 
plemented  in  discrete-time,  there  is  an  upper  bound  on  the  norm  of  K  with 
reference  to  the  adopted  sampling  time. 


Example  3.4 

Consider  the  anthropomorphic  arm;  a  shoulder  singularity  occurs  whenever  C12C2  + 
C3C23  =  0  (Fig.  3.6).  In  this  configuration,  the  transpose  of  the  Jacobian  in  (3.38)  is 

0  0  O' 

-cr(a2S2  +  a3S23)  -si  (a2s2  +  a3s23)  0 

— a3ClS23  —C13S1S23  C13C23 . 

By  computing  the  null  space  of  Jj>,  if  vx,  vy  and  vz  denote  the  components  of  vector 
v  along  the  axes  of  the  base  frame,  one  has  the  result 


vx  tan 

13  Notice  that,  anyhow,  in  case  of  kinematic  singularities,  it  is  necessary  to  resort 
to  an  inverse  kinematics  scheme  that  does  not  require  inversion  of  the  Jacobian. 

14  Notice  that  the  negative  definite  term  is  a  quadratic  function  of  the  error,  while 
the  other  term  is  a  linear  function  of  the  error.  Therefore,  for  an  error  of  very 
small  norm,  the  linear  term  prevails  over  the  quadratic  term,  and  the  norm  of  K 
should  be  increased  to  reduce  the  norm  of  e  as  much  as  possible. 


3.7  Inverse  Kinematics  Algorithms  137 


implying  that  the  direction  of  AT( Jj>)  coincides  with  the  direction  orthogonal  to  the 
plane  of  the  structure  (Fig.  3.13).  The  Jacobian  transpose  algorithm  gets  stuck  if, 
with  K  diagonal  and  having  all  equal  elements,  the  desired  position  is  along  the  line 
normal  to  the  plane  of  the  structure  at  the  intersection  with  the  wrist  point.  On  the 
other  hand,  the  end-effector  cannot  physically  move  from  the  singular  configuration 
along  such  a  line.  Instead,  if  the  prescribed  path  has  a  non-null  component  in  the 
plane  of  the  structure  at  the  singularity,  algorithm  convergence  is  ensured,  since  in 
that  case  Ke  ^  A /"(Jp). 


In  summary,  the  algorithm  based  on  the  computation  of  the  Jacobian 
transpose  provides  a  computationally  efficient  inverse  kinematics  method  that 
can  be  utilized  also  for  paths  crossing  kinematic  singularities. 


3.7.3  Orientation  Error 

The  inverse  kinematics  algorithms  presented  in  the  above  sections  utilize  the 
analytical  Jacobian  since  they  operate  on  error  variables  (position  and  orien¬ 
tation)  that  are  defined  in  the  operational  space. 

For  what  concerns  the  position  error,  it  is  obvious  that  its  expression  is 
given  by 

ep=Pd~Pe(<l)  (3.78) 

where  pd  and  pe  denote  respectively  the  desired  and  computed  end-effector 
positions.  Further,  its  time  derivative  is 


ep  =  Pd-Pe ■  (3-79) 

On  the  other  hand,  for  what  concerns  the  orientation  error,  its  expression 
depends  on  the  particular  representation  of  end-effector  orientation,  namely, 
Euler  angles,  angle  and  axis,  and  unit  quaternion. 

Euler  angles 

The  orientation  error  is  chosen  according  to  an  expression  formally  analogous 
to  (3.78),  i.e., 

eo  =  ~  </>e(<7)  (3-80) 

where  cj)d  and  <f>e  denote  respectively  the  desired  and  computed  set  of  Euler 
angles.  Further,  its  time  derivative  is 

eo  =  </>d  -  0e-  (3-81) 

Therefore,  assuming  that  neither  kinematic  nor  representation  singularities 
occur,  the  Jacobian  inverse  solution  for  a  nonredundant  manipulator  is  derived 
from  (3.70),  i.e., 
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<?  = 


pd  +  Kpep 

4>d  +  Koeo 


(3.82) 


where  Kp  and  Ko  are  positive  definite  matrices. 

As  already  pointed  out  in  Sect.  2.10  for  computation  of  the  direct  kinemat¬ 
ics  function  in  the  form  (2.82),  the  determination  of  the  orientation  variables 
from  the  joint  variables  is  not  easy  except  for  simple  cases  (see  Example  2.5). 
To  this  end,  it  is  worth  recalling  that  computation  of  the  angles  </>e,  in  a 
minimal  representation  of  orientation,  requires  computation  of  the  rotation 
matrix  Re  =  [ne  se  ae  ];  in  fact,  only  the  dependence  of  Re  on  q  is  known 
in  closed  form,  but  not  that  of  <pe  on  q.  Further,  the  use  of  inverse  func¬ 
tions  (Atan2)  in  (2.19),  (2.22)  involves  a  non-negligible  complexity  in  the 
computation  of  the  analytical  Jacobian,  and  the  occurrence  of  representation 
singularities  constitutes  another  drawback  for  the  orientation  error  based  on 
Euler  angles. 

Different  kinds  of  remarks  are  to  be  made  about  the  way  to  assign  a  time 
profile  for  the  reference  variables  (j)d  chosen  to  represent  end-effector  orienta¬ 
tion.  The  most  intuitive  way  to  specify  end-effector  orientation  is  to  refer  to 
the  orientation  of  the  end-effector  frame  (nd,  sd,  ad)  with  respect  to  the  base 
frame.  Given  the  limitations  pointed  out  in  Sect.  2.10  about  guaranteeing  or¬ 
thonormality  of  the  unit  vectors  along  time,  it  is  necessary  first  to  compute 
the  Euler  angles  corresponding  to  the  initial  and  final  orientation  of  the  end- 
effector  frame  via  (2.19),  (2.22);  only  then  a  time  evolution  can  be  generated. 
Such  solutions  will  be  presented  in  Chap.  4. 

A  radical  simplification  of  the  problem  at  issue  can  be  obtained  for  manip¬ 
ulators  having  a  spherical  wrist.  Section  2.12.2  pointed  out  the  possibility  to 
solve  the  inverse  kinematics  problem  for  the  position  part  separately  from  that 
for  the  orientation  part.  This  result  also  has  an  impact  at  algorithmic  level.  In 
fact,  the  implementation  of  an  inverse  kinematics  algorithm  for  determining 
the  joint  variables  influencing  the  wrist  position  allows  the  computation  of 
the  time  evolution  of  the  wrist  frame  Rw(t).  Hence,  once  the  desired  time 
evolution  of  the  end-effector  frame  Rd(t)  is  given,  it  is  sufficient  to  compute 
the  Euler  angles  ZYZ  from  the  matrix  R,JvRd  by  applying  (2.19).  As  shown 
in  Sect.  2.12.5,  these  angles  are  directly  the  joint  variables  of  the  spherical 
wrist.  See  also  Problem  3.14. 

The  above  considerations  show  that  the  inverse  kinematics  algorithms 
based  on  the  analytical  Jacobian  are  effective  for  kinematic  structures  having 
a  spherical  wrist  which  are  of  significant  interest.  For  manipulator  structures 
which  cannot  be  reduced  to  that  class,  it  may  be  appropriate  to  reformulate 
the  inverse  kinematics  problem  on  the  basis  of  a  different  definition  of  the 
orientation  error. 


3.7  Inverse  Kinematics  Algorithms  139 


Angle  and  axis 

If  Rd  =  [ n,i  Sd  ad]  denotes  the  desired  rotation  matrix  of  the  end-effector 
frame  and  Re  =  [  ne  se  ae }  the  rotation  matrix  that  can  be  computed 
from  the  joint  variables,  the  orientation  error  between  the  two  frames  can  be 
expressed  as 

eo  =  rsin$  (3.83) 

where  id  and  r  identify  the  angle  and  axis  of  the  equivalent  rotation  that  can 
be  deduced  from  the  matrix 


R(0,r)  =  RdR*(q),  (3.84) 

describing  the  rotation  needed  to  align  R  with  Rd.  Notice  that  (3.83)  gives  a 
unique  relationship  for  — 7r/2  <  i9  <  tt/2.  The  angle  i9  represents  the  magni¬ 
tude  of  an  orientation  error,  and  thus  the  above  limitation  is  not  restrictive 
since  the  tracking  error  is  typically  small  for  an  inverse  kinematics  algorithm. 

By  comparing  the  off-diagonal  terms  of  the  expression  of  R(id,  r )  in  (2.25) 
with  the  corresponding  terms  resulting  on  the  right-hand  side  of  (3.84),  it  can 
be  found  that  a  functional  expression  of  the  orientation  error  in  (3.83)  is  (see 
Problem  3.16) 

eo  =  ^( ne(q )  x  nd  +  se{q )  x  sd  +  ae(q )  x  ad );  (3.85) 

the  limitation  on  id  is  transformed  in  the  condition  njnd  >  0,  s^sd  >  0, 
a^ad  >  0. 

Differentiating  (3.85)  with  respect  to  time  and  accounting  for  the  expres- 


sion  of  the  columns  of  the  derivative  of  a  rotation  matrix  in  (3.8)  gives  (see 

Problem  3.19) 

e0  =  LT LCd  -  Luje 

(3.86) 

where 

-^(S(nd)S(ne)  +  S(sd)S(se)  +  S(ad)S(ae)). 

L  = 

(3.87) 

At  this  point,  by  exploiting  the  relations  (3.2),  (3.3)  of  the  geometric  Jacobian 
expressing  pe  and  u)e  as  a  function  of  q ,  (3.79),  (3.86)  become 


eP 

Pd~Jp(q)<i 

Pd 

I  O 

eo_ 

_LTud  -  LJ0(q)q 

LTud_ 

°  L. 

(3.88) 


The  expression  in  (3.88)  suggests  the  possibility  of  devising  inverse  kinematics 
algorithms  analogous  to  the  ones  derived  above,  but  using  the  geometric  Ja¬ 
cobian  in  place  of  the  analytical  Jacobian.  For  instance,  the  Jacobian  inverse 
solution  for  a  nonredundant  nonsingular  manipulator  is 


Pd  +  Kpep 

L1  (LTu,d  +  K0eo) 


q  =  J  1{q) 


(3.89) 
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It  is  worth  remarking  that  the  inverse  kinematics  solution  based  on  (3.89) 
is  expected  to  perform  better  than  the  solution  based  on  (3.82)  since  it  uses 
the  geometric  Jacobian  in  lieu  of  the  analytical  Jacobian,  thus  avoiding  the 
occurrence  of  representation  singularities. 


Unit  quaternion 


In  order  to  devise  an  inverse  kinematics  algorithm  based  on  the  unit  quater¬ 
nion. ,  a  suitable  orientation  error  should  be  defined.  Let  Qd  =  {qd,  £d}  and 
Qe  =  {j]e,£e}  represent  the  quaternions  associated  with  Rd  and  Rei  re¬ 
spectively.  The  orientation  error  can  be  described  by  the  rotation  matrix 
RdR J  and,  in  view  of  (2.37),  can  be  expressed  in  terms  of  the  quaternion 
AQ  =  {Arj,  Ae}  where 

AQ=Qd*Q-\  (3.90) 

It  can  be  recognized  that  AQ  =  {1, 0}  if  and  only  if  Re  and  Rd  are  aligned. 
Hence,  it  is  sufficient  to  define  the  orientation  error  as 


eQ  =  Ae  =  r)e(q)ed  -  rjdee{q)  -  S{ed)ee{q),  (3.91) 


where  the  skew-symmetric  operator  S(-)  has  been  used.  Notice,  however,  that 
the  explicit  computation  of  qe  and  ee  from  the  joint  variables  is  not  possible 
but  it  requires  the  intermediate  computation  of  the  rotation  matrix  Re  that 
is  available  from  the  manipulator  direct  kinematics;  then,  the  quaternion  can 
be  extracted  using  (2.34). 

At  this  point,  a  Jacobian  inverse  solution  can  be  computed  as 


q  =  J  1{q) 


Pd  +  KPeP 
ud  +  Kq^o 


(3.92) 


where  noticeably  the  geometric  Jacobian  has  been  used.  Substituting  (3.92) 
into  (3.4)  gives  (3.79)  and 


oj d  —  oje  +  K0e0  —  0.  (3.93) 

It  should  be  observed  that  now  the  orientation  error  equation  is  nonlinear 
in  eo  since  it  contains  the  end-effector  angular  velocity  error  instead  of  the 
time  derivative  of  the  orientation  error.  To  this  end,  it  is  worth  considering 
the  relationship  between  the  time  derivative  of  the  quaternion  Qe  and  the 
angular  velocity  u>e.  This  can  be  found  to  be  (see  Problem  3.19) 

Ve  =  ~^uje  (3.94) 

ee  =  ^  {Pels  -  S{ee))  u>e  (3.95) 

which  is  the  so-called  quaternion  propagation.  A  similar  relationship  holds 
between  the  time  derivative  of  Qd  and  u>d. 
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To  study  stability  of  system  (3.93),  consider  the  positive  definite  Lyapunov 
function  candidate 


V  =  (r/a-  Ve)2  +  (ed  -  ee)T{ed  -  ee).  (3.96) 

In  view  of  (3.94),  (3.95),  differentiating  (3.96)  with  respect  to  time  and  ac¬ 
counting  for  (3.93)  yields  (see  Problem  3.20) 

V  =  -eT0K0e0  (3.97) 


which  is  negative  definite,  implying  that  ep  converges  to  zero. 

In  summary,  the  inverse  kinematics  solution  based  on  (3.92)  uses  the  geo¬ 
metric  Jacobian  as  the  solution  based  on  (3.89)  but  is  computationally  lighter. 


3.7.4  Second-order  Algorithms 

The  above  inverse  kinematics  algorithms  can  be  defined  as  first-order  algo¬ 
rithms,  in  that  they  allow  the  inversion  of  a  motion  trajectory,  specified  at 
the  end-effector  in  terms  of  of  position  and  orientation,  into  the  equivalent 
joint  positions  and  velocities. 

Nevertheless,  as  will  be  seen  in  Chap.  8,  for  control  purposes  it  may  be 
necessary  to  invert  a  motion  trajectory  specified  in  terms  of  position,  velocity 
and  acceleration.  On  the  other  hand,  the  manipulator  is  inherently  a  second- 
order  mechanical  system,  as  will  be  revealed  by  the  dynamic  model  to  be 
derived  in  Chap.  7. 

The  time  differentiation  of  the  differential  kinematics  equation  (3.62)  leads 
to 

xe  =  JA(q)q  +  JA(q,q)q  (3.98) 

which  gives  the  relationship  between  the  joint  space  accelerations  and  the 
operational  space  accelerations. 

Under  the  assumption  of  a  square  and  non-singular  matrix  J  a,  the  second- 
order  differential  kinematics  (3.98)  can  be  inverted  in  terms  of  the  joint  ac¬ 
celerations 

Q  =  Ja1^)  (*e  -  JA(q,q)qj  .  (3.99) 

The  numerical  integration  of  (3.99)  to  reconstruct  the  joint  velocities  and 
positions  would  unavoidably  lead  to  a  drift  of  the  solution;  therefore,  similarly 
to  the  inverse  kinematics  algorithm  with  the  Jacobian  inverse,  it  is  worth 
considering  the  error  defined  in  (3.68)  along  with  its  derivative 

e  =  xd  —  xe  (3.100) 


which,  in  view  of  (3.98),  yields 


e  =  xd-  JA{q)q  -  JA{q,q)q- 


(3.101) 
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xd 


Fig.  3.14.  Block  scheme  of  the  second-order  inverse  kinematics  algorithm  with 
Jacobian  inverse 


At  this  point,  it  is  advisable  to  choose  the  joint  acceleration  vector  as 

q  =  J~^(q)  (xd  +  KDe  +  KPe  -  JA(q,  <?)<?)  (3.102) 

where  Kp,  and  Kp  are  positive  definite  (typically  diagonal)  matrices.  Sub¬ 
stituting  (3.102)  into  (3.101)  leads  to  the  equivalent  linear  error  system 

e  +  Kp,e  +  KPe  =  0  (3.103) 

which  is  asymptotically  stable:  the  error  tends  to  zero  along  the  trajectory  with 
a  convergence  speed  depending  on  the  choice  of  the  matrices  Kp  e  Kp,.  The 
second-order  inverse  kinematics  algorithm  is  illustrated  in  the  block  scheme 
of  Fig.  3.14. 

In  the  case  of  a  redundant  manipulator,  the  generalization  of  (3.102)  leads 
to  an  algorithmic  solution  based  on  the  Jacobian  pseudo-inverse  of  the  kind 

q  =  J\  (xd  +  KDe  +  KPe  -  j A(q,  q)qj  +  {In  -  JAJ A)q0  (3.104) 

where  the  vector  q0  represents  arbitrary  joint  accelerations  which  can  be  cho¬ 
sen  so  as  to  (locally)  optimize  an  objective  function  like  those  considered  in 
Sect.  3.5.1. 

As  for  the  first-order  inverse  kinematics  algorithms,  it  is  possible  to  con¬ 
sider  other  expressions  for  the  orientation  error  which,  unlike  the  Euler  angles, 
refer  to  an  angle  and  axis  description,  else  to  the  unit  quaternion. 
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3.7.5  Comparison  Among  Inverse  Kinematics  Algorithms 


In  order  to  make  a  comparison  of  performance  among  the  inverse  kinematics 
algorithms  presented  above,  consider  the  3-link  planar  arm  in  Fig.  2.20  whose 
link  lengths  are  aq  =  aq  =  <23  =  0.5  m.  The  direct  kinematics  for  this  arm  is 
given  by  (2.83),  while  its  Jacobian  can  be  found  from  (3.35)  by  considering 
the  3  non-null  rows  of  interest  for  the  operational  space. 

Let  the  arm  be  at  the  initial  posture  q  =  [tt  — 7t/2  — 7r/2]Trad,  corre¬ 
sponding  to  the  end-effector  pose:  p  =  [  0  0.5  }T  m,  </>  =  0  rad.  A  circular  path 

of  radius  0.25  m  and  centre  at  (0.25, 0.5)  m  is  assigned  to  the  end-effector.  Let 
the  motion  trajectory  be 


Pd(t) 


0.25(1  —  cos  7 rf) 
0.25(2  +  sin7rf) 


0  <  t  <  4; 


i.e.,  the  end-effector  has  to  make  two  complete  circles  in  a  time  of  2  s  per 
circle.  As  regards  end-effector  orientation,  initially  it  is  required  to  follow  the 
trajectory 

=  sin  0  <  t  <  4; 

i.e.,  the  end-effector  has  to  attain  a  different  orientation  (^  =  0.5  rad)  at  the 
end  of  the  two  circles. 

The  inverse  kinematics  algorithms  were  implemented  on  a  computer  by 
adopting  the  Euler  numerical  integration  scheme  (3.48)  with  an  integration 
time  At  =  1  ms. 

At  first,  the  inverse  kinematics  along  the  given  trajectory  has  been  per¬ 
formed  by  using  (3.47).  The  results  obtained  in  Fig.  3.15  show  that  the  norm 
of  the  position  error  along  the  whole  trajectory  is  bounded;  at  steady  state, 
after  t  =  4,  the  error  sets  to  a  constant  value  in  view  of  the  typical  drift,  of 
open-loop  schemes.  A  similar  drift  can  be  observed  for  the  orientation  error. 

Next,  the  inverse  kinematics  algorithm  based  on  (3.70)  using  the  Jacobian 
inverse  has  been  used,  with  the  matrix  gain  K  =  diag{500, 500, 100}.  The 
resulting  joint  positions  and  velocities  as  well  as  the  tracking  errors  are  shown 
in  Fig.  3.16.  The  norm  of  the  position  error  is  radically  decreased  and  con¬ 
verges  to  zero  at  steady  state,  thanks  to  the  closed-loop  feature  of  the  scheme; 
the  orientation  error,  too,  is  decreased  and  tends  to  zero  at  steady  state. 

On  the  other  hand,  if  the  end-effector  orientation  is  not  constrained,  the 
operational  space  becomes  two-dimensional  and  is  characterized  by  the  first 
two  rows  of  the  direct  kinematics  in  (2.83)  as  well  as  by  the  Jacobian  in  (3.36); 
a  redundant  DOF  is  then  available.  Hence,  the  inverse  kinematics  algorithm 
based  on  (3.72)  using  the  Jacobian  pseudo-inverse  has  been  used  with  K  = 
diag{500, 500}.  If  redundancy  is  not  exploited  (q0  =  0) ,  the  results  in  Fig.  3. 17 
reveal  that  position  tracking  remains  satisfactory  and,  of  course,  the  end- 
effector  orientation  freely  varies  along  the  given  trajectory. 

With  reference  to  the  previous  situation,  the  use  of  the  Jacobian  transpose 
algorithm  based  on  (3.76)  with  K  =  diag{500,  500}  gives  rise  to  a  tracking 
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Fig.  3.15.  Time  history  of  the  norm  of  end-effector  position  error  and  orientation 
error  with  the  open-loop  inverse  Jacobian  algorithm 


joint  pos  joint  vel 


[s]  [S] 

Fig.  3.16.  Time  history  of  the  joint  positions  and  velocities,  and  of  the  norm  of  end- 
effector  position  error  and  orientation  error  with  the  closed-loop  inverse  Jacobian 
algorithm 


error  (Fig.  3.18)  which  is  anyhow  bounded  and  rapidly  tends  to  zero  at  steady 
state. 

In  order  to  show  the  capability  of  handling  the  degree  of  redundancy,  the 
algorithm  based  on  (3.72)  with  q0  ^  0  has  been  used;  two  types  of  constraints 
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Fig.  3.17.  Time  history  of  the  norm  of  end-effector  position  error  and  orientation 
with  the  Jacobian  pseudo-inverse  algorithm 


pos  error  norm  orien 


Fig.  3.18.  Time  history  of  the  norm  of  end-effector  position  error  and  orientation 
with  the  Jacobian  transpose  algorithm 


have  been  considered  concerning  an  objective  function  to  locally  maximize 
according  to  the  choice  (3.55).  The  first  function  is 

2,^3)  =  1,(4 +  4) 

that  provides  a  manipulability  measure.  Notice  that  such  a  function  is  compu¬ 
tationally  simpler  than  the  function  in  (3.56),  but  it  still  describes  a  distance 
from  kinematic  singularities  in  an  effective  way.  The  gain  in  (3.55))  has  been 
set  to  fc0  =  50.  In  Fig.  3.19,  the  joint  trajectories  are  reported  for  the  two 
cases  with  and  without  ( =  0)  constraint.  The  addition  of  the  constraint 
leads  to  having  coincident  trajectories  for  Joints  2  and  3.  The  manipulability 
measure  in  the  constrained  case  (continuous  line)  attains  larger  values  along 
the  trajectory  compared  to  the  unconstrained  case  (dashed  line).  It  is  worth 
underlining  that  the  tracking  position  error  is  practically  the  same  in  the  two 
cases  (Fig.  3.17),  since  the  additional  joint  velocity  contribution  is  projected 
in  the  null  space  of  the  Jacobian  so  as  not  to  alter  the  performance  of  the 
end-effector  position  task. 

Finally,  it  is  worth  noticing  that  in  the  constrained  case  the  resulting  joint 
trajectories  are  cyclic,  i.e. ,  they  take  on  the  same  values  after  a  period  of 
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joint  pos 


joint  pos 


manip 


Fig.  3.19.  Time  history  of  the  joint  positions,  the  norm  of  end-effector  position 
error,  and  the  manipulability  measure  with  the  Jacobian  pseudo-inverse  algorithm 
and  manipulability  constraint;  upper  left :  with  the  unconstrained  solution,  upper 
right :  with  the  constrained  solution 


the  circular  path.  This  does  not  happen  for  the  unconstrained  case,  since  the 
internal  motion  of  the  structure  causes  the  arm  to  be  in  a  different  posture 
after  one  circle. 

The  second  objective  function  considered  is  the  distance  from  mechanical 
joint  limits  in  (3.57).  Specifically,  it  is  assumed  what  follows:  the  first  joint 
does  not  have  limits  (qim  =  —2tt,  q\  m  =  27 r),  the  second  joint  has  limits  q2m  = 
— 7t/2,  q2M  =  7t/2,  and  the  third  joint  has  limits  q^m  =  —  37r/2,  q^M  =  — 7t/2. 
It  is  not  difficult  to  verify  that,  in  the  unconstrained  case,  the  trajectories  of 
Joints  2  and  3  in  Fig.  3.19  violate  the  respective  limits.  The  gain  in  (3.55) 
has  been  set  to  ko  =  250.  The  results  in  Fig.  3.20  show  the  effectiveness  of 
the  technique  with  utilization  of  redundancy,  since  both  Joints  2  and  3  tend 
to  invert  their  motion  —  with  respect  to  the  unconstrained  trajectories  in 
Fig.  3.19  —  and  keep  far  from  the  minimum  limit  for  Joint  2  and  the  maximum 
limit  for  Joint  3,  respectively.  Such  an  effort  does  not  appreciably  affect  the 
position  tracking  error,  whose  norm  is  bounded  anyhow  within  acceptable 
values. 
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joint  3  pos  x  jQ-4  pos  error  norm 


Fig.  3.20.  Time  history  of  the  joint  positions  and  the  norm  of  end-effector  position 
error  with  the  Jacobian  pseudo-inverse  algorithm  and  joint  limit  constraint  (joint 
limits  are  denoted  by  dashed  lines) 


3.8  Statics 

The  goal  of  statics  is  to  determine  the  relationship  between  the  generalized 
forces  applied  to  the  end-effector  and  the  generalized  forces  applied  to  the 
joints  —  forces  for  prismatic  joints,  torques  for  revolute  joints  —  with  the 
manipulator  at  an  equilibrium  configuration. 

Let  r  denote  the  (n  x  1)  vector  of  joint  torques  and  7  the  (r  x  1)  vector 
of  end-effector  forces15  where  r  is  the  dimension  of  the  operational  space  of 
interest. 

The  application  of  the  principle  of  virtual  work  allows  the  determination 
of  the  required  relationship.  The  mechanical  manipulators  considered  are  sys¬ 
tems  with  time-invariant,  holonomic  constraints,  and  thus  their  configurations 
depend  only  on  the  joint  variables  q  and  not  explicitly  on  time.  This  implies 
that  virtual  displacements  coincide  with  elementary  displacements. 

Consider  the  elementary  works  performed  by  the  two  force  systems.  As  for 
the  joint  torques,  the  elementary  work  associated  with  them  is 

dWT  =  rTdq.  (3.105) 

15  Hereafter,  generalized  forces  at  the  joints  are  often  called  torques,  while  general¬ 
ized  forces  at  the  end-effector  are  often  called  forces. 
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As  for  the  end-effector  forces  7,  if  the  force  contributions  fe  are  separated  by 
the  moment  contributions  pe,  the  elementary  work  associated  with  them  is 

dW1  =  fgdpe  +  p^Ljedt,  (3.106) 

where  dpe  is  the  linear  displacement  and  u)edt  is  the  angular  displacement16 
By  accounting  for  the  differential  kinematics  relationship  in  (3.4),  (3.5), 
the  relation  (3.106)  can  be  rewritten  as 

dW~r  =  feJp(q)dq  +  PeJo(q)dq  (3.107) 

=  7  eJ(q)dq 

where  7e  =  [  /J  pj  }T .  Since  virtual  and  elementary  displacements  coincide, 
the  virtual  works  associated  with  the  two  force  systems  are 

SWT  =  tt  5  q  (3.108) 

5W~y  = -yjj(q)6q,  (3.109) 

where  S  is  the  usual  symbol  to  indicate  virtual  quantities. 

According  to  the  principle  of  virtual  work,  the  manipulator  is  at  static 
equilibrium  if  and  only  if 


SWT  =  5W1  WSq,  (3.110) 

i.e.,  the  difference  between  the  virtual  work  of  the  joint  torques  and  the  virtual 
work  of  the  end-effector  forces  must  be  null  for  all  joint  displacements. 

From  (3.109),  notice  that  the  virtual  work  of  the  end-effector  forces  is 
null  for  any  displacement  in  the  null  space  of  J .  This  implies  that  the  joint 
torques  associated  with  such  displacements  must  be  null  at  static  equilibrium. 
Substituting  (3.108),  (3.109)  into  (3.110)  leads  to  the  notable  result 

T  =  jT(<l)  le  (3.111) 

stating  that  the  relationship  between  the  end-effector  forces  and  the  joint 
torques  is  established  by  the  transpose  of  the  manipulator  geometric  Jacobian. 

3.8.1  Kineto-Statics  Duality 

The  statics  relationship  in  (3.111),  combined  with  the  differential  kinematics 
equation  in  (3.45),  points  out  a  property  of  kineto- statics  duality.  In  fact,  by 
adopting  a  representation  similar  to  that  of  Fig.  3.7  for  differential  kinematics, 
one  has  that  (Fig.  3.21): 

•  The  range  space  of  JT  is  the  subspace  7 Z(JT)  in  IR™  of  the  joint  torques 
that  can  balance  the  end-effector  forces,  in  the  given  manipulator  posture. 


16 


The  angular  displacement  has  been  indicated  by  ujedt  in  view  of  the  problems  of 
integrability  of  u>e  discussed  in  Sect.  3.6. 
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Fig.  3.21.  Mapping  between  the  end-effector  force  space  and  the  joint  torque  space 


•  The  null  space  of  JT  is  the  subspace  Af(JT)  in  IRr  of  the  end-effector  forces 
that  do  not  require  any  balancing  joint  torques,  in  the  given  manipulator 
posture. 

It  is  worth  remarking  that  the  end-effector  forces  -ye  €  Af(JT)  are  entirely 
absorbed  by  the  structure  in  that  the  mechanical  constraint  reaction  forces 
can  balance  them  exactly.  Hence,  a  manipulator  at  a  singular  configuration 
remains  in  the  given  posture  whatever  end-effector  force  7e  is  applied  so  that 
7e  e  Af(JT). 

The  relations  between  the  two  subspaces  are  established  by 

A f(J)  =  K^(Jt)  7 Z(J)  =  Af±(JT) 

and  then,  once  the  manipulator  Jacobian  is  known,  it  is  possible  to  charac¬ 
terize  completely  differential  kinematics  and  statics  in  terms  of  the  range  and 
null  spaces  of  the  Jacobian  and  its  transpose. 

On  the  basis  of  the  above  duality,  the  inverse  kinematics  scheme  with  the 
Jacobian  transpose  in  Fig.  3.12  admits  an  interesting  physical  interpretation. 
Consider  a  manipulator  with  ideal  dynamics  t  =  q  (null  masses  and  unit 
viscous  friction  coefficients);  the  algorithm  update  law  q  =  JT  Ke  plays  the 
role  of  a  generalized  spring  of  stiffness  constant  K  generating  a  force  Ke  that 
pulls  the  end-effector  towards  the  desired  posture  in  the  operational  space. 
If  this  manipulator  is  allowed  to  move,  e.g.,  in  the  case  Ke  £  Af(JT),  the 
end-effector  attains  the  desired  posture  and  the  corresponding  joint  variables 
are  determined. 

3.8.2  Velocity  and  Force  Transformation 

The  kineto-statics  duality  concept  presented  above  can  be  useful  to  character¬ 
ize  the  transformation  of  velocities  and  forces  between  two  coordinate  frames. 

Consider  a  reference  coordinate  frame  Oq-XqUoZo  and  a  rigid  body  moving 
with  respect  to  such  a  frame.  Then  let  Oi-x\y\Zi  and  O2-X2IJ2Z2  be  two 
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Fig.  3.22.  Representation  of  linear  and  angular  velocities  in  different  coordinate 
frames  on  the  same  rigid  body 


coordinate  frames  attached  to  the  body  (Fig.  3.22).  The  relationships  between 
translational  and  rotational  velocities  of  the  two  frames  with  respect  to  the 
reference  frame  are  given  by 


w2  =  Wi 

P-2  =  Pi  +  X  r12- 


By  exploiting  the  skew-symmetric  operator  S(-)  in  (3.9),  the  above  relations 
can  be  compactly  written  as 


P  2 

I 

-S(r12y 

Pi 

U>2 

O 

I 

Wl_ 

(3.112) 


All  vectors  in  (3.112)  are  meant  to  be  referred  to  the  reference  frame  Oq- 
xoyoZQ.  On  the  other  hand,  if  vectors  are  referred  to  their  own  frames,  it 
is 

r  12  =  Rir\2 

and  also 


Pi  —  RiPi  P2  —  R2P2  —  R1R2P2 

cji  —  R\i <jj  2  =  R2^2  —  RiR^Ldo- 


Accounting  for  (3.112)  and  (3.11)  gives 

R\R\p22  =  R\p\  -  RiSirlJR^R^l 
RiR-^uj^  =  R\uj^. 


Eliminating  the  dependence  on  Ri,  which  is  premultiplied  to  each  term  on 
both  sides  of  the  previous  relations,  yields17 


(M(M 

3 

Ri  -RlS{r\2) 


O 


R\ 


pV 

u\_ 

(3.113) 


17  Recall  that  RT R  =  I,  as  in  (2.4). 
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giving  the  sought  general  relationship  of  velocity  transformation  between  two 
frames. 

It  may  be  observed  that  the  transformation  matrix  in  (3.113)  plays  the 
role  of  a  true  Jacobian,  since  it  characterizes  a  velocity  transformation,  and 
thus  (3.113)  may  be  shortly  written  as 

v22  =  j\v\.  (3.114) 

At  this  point,  by  virtue  of  the  kineto-statics  duality,  the  force  transformation 
between  two  frames  can  be  directly  derived  in  the  form 

7i  =  J\Tll  (3-115) 


which  can  be  detailed  into18 

/ii  =  r  r\  oi  \fV 

A4!  -  -  (4*12 )  R2  R"2  -  -^2- 


(3.116) 


Finally,  notice  that  the  above  analysis  is  instantaneous  in  that,  if  a  coordinate 
frame  varies  with  respect  to  the  other,  it  is  necessary  to  recompute  the  Jaco¬ 
bian  of  the  transformation  through  the  computation  of  the  related  rotation 
matrix  of  one  frame  with  respect  to  the  other. 


3.8.3  Closed  Chain 

As  discussed  in  Sect.  2.8.3,  whenever  the  manipulator  contains  a  closed  chain, 
there  is  a  functional  relationship  between  the  joint  variables.  In  particular, 
the  closed  chain  structure  is  transformed  into  a  tree-structured  open  chain  by 
virtually  cutting  the  loop  at  a  joint.  It  is  worth  choosing  such  a  cut  joint  as 
one  of  the  unactuated  joints.  Then,  the  constraints  (2.59)  or  (2.60)  should  be 
solved  for  a  reduced  number  of  joint  variables,  corresponding  to  the  DOFs  of 
the  chain.  Therefore,  it  is  reasonable  to  assume  that  at  least  such  independent 
joints  are  actuated,  while  the  others  may  or  may  not  be  actuated.  Let  qQ  = 
{ qf  qf]  denote  the  vector  of  joint  variables  of  the  tree-structured  open 
chain,  where  qa  and  qu  are  the  vectors  of  actuated  and  unactuated  joint 
variables,  respectively.  Assume  that  from  the  above  constraints  it  is  possible 
to  determine  a  functional  expression 

Qu  =  Qu(Qa)-  (3.117) 

Time  differentiation  of  (3.117)  gives  the  relationship  between  joint  velocities 
in  the  form 

Qo  =  Tqa  (3.118) 

where 

I 

Y  =  dq^  (3.119) 

.  dqa  _ 

18  The  skew-symmetry  property  S  +  ST  =  O  is  utilized. 
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is  the  transformation  matrix  between  the  two  vectors  of  joint  velocities,  which 
in  turn  plays  the  role  of  a  Jacobian. 

At  this  point,  according  to  an  intuitive  kineto-statics  duality  concept,  it  is 
possible  to  describe  the  transformation  between  the  corresponding  vectors  of 
joint  torques  in  the  form 

r  a  =  Ttt0  (3.120) 

where  t0  =  [  t„]T,  with  obvious  meaning  of  the  quantities. 


Example  3.5 

Consider  the  parallelogram  arm  of  Sect.  2.9.2.  On  the  assumption  to  actuate  the 
two  Joints  1'  and  1"  at  the  base,  it  is  qa  =  [i?i'  ,&v,]T  and  qu  =  [$2'  $3' ]T. 

Then,  using  (2.64),  the  transformation  matrix  in  (3.119)  is 


1 


1 


Hence,  in  view  of  (3.120),  the  torque  vector  of  the  actuated  joints  is 


Ta 


tv  -  T2 >  +  Ty 
T\n  +  Ty  ~  Ty 


(3.121) 


while  obviously  tu  =  [ 0  0]T  in  agreement  with  the  fact  that  both  Joints  2 '  and  3; 

are  unactuated. 


3.9  Manipulability  Ellipsoids 

The  differential  kinematics  equation  in  (3.45)  and  the  statics  equation  in 
(3.111),  together  with  the  duality  property,  allow  the  definition  of  indices  for 
the  evaluation  of  manipulator  performance.  Such  indices  can  be  helpful  both 
for  mechanical  manipulator  design  and  for  determining  suitable  manipulator 
postures  to  execute  a  given  task  in  the  current  configuration. 

First,  it  is  desired  to  represent  the  attitude  of  a  manipulator  to  arbitrarily 
change  end-effector  position  and  orientation.  This  capability  is  described  in 
an  effective  manner  by  the  velocity  manipulability  ellipsoid. 

Consider  the  set  of  joint  velocities  of  constant  (unit)  norm 

qTq  =  1;  (3.122) 

this  equation  describes  the  points  on  the  surface  of  a  sphere  in  the  joint  ve¬ 
locity  space.  It  is  desired  to  describe  the  operational  space  velocities  that  can 
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be  generated  by  the  given  set  of  joint  velocities,  with  the  manipulator  in  a 
given  posture.  To  this  end,  one  can  utilize  the  differential  kinematics  equation 
in  (3.45)  solved  for  the  joint  velocities;  in  the  general  case  of  a  redundant  ma¬ 
nipulator  (r  <  n)  at  a  nonsingular  configuration,  the  minimum-norm  solution 
q  =  j\q)ve  can  be  considered  which,  substituted  into  (3.122),  yields 

vl(Jn{q)J]{q))ve  =  1. 

Accounting  for  the  expression  of  the  pseudo-inverse  of  J  in  (3.52)  gives 

VT(J(q)JT(q)y1ve  =  1,  (3.123) 

which  is  the  equation  of  the  points  on  the  surface  of  an  ellipsoid  in  the  end- 
effector  velocity  space. 

The  choice  of  the  minimum-norm  solution  rules  out  the  presence  of  internal 
motions  for  the  redundant  structure.  If  the  general  solution  (3.54)  is  used  for 
q,  the  points  satisfying  (3.122)  are  mapped  into  points  inside  the  ellipsoid 
whose  surface  is  described  by  (3.123). 

For  a  nonredundant  manipulator,  the  differential  kinematics  solution  (3.47) 
is  used  to  derive  (3.123);  in  this  case  the  points  on  the  surface  of  the  sphere  in 
the  joint  velocity  space  are  mapped  into  points  on  the  surface  of  the  ellipsoid 
in  the  end-effector  velocity  space. 

Along  the  direction  of  the  major  axis  of  the  ellipsoid,  the  end-effector  can 
move  at  large  velocity,  while  along  the  direction  of  the  minor  axis  small  end- 
effector  velocities  are  obtained.  Further,  the  closer  the  ellipsoid  is  to  a  sphere 
—  unit  eccentricity  —  the  better  the  end-effector  can  move  isotropically  along 
all  directions  of  the  operational  space.  Hence,  it  can  be  understood  why  this 
ellipsoid  is  an  index  characterizing  manipulation  ability  of  the  structure  in 
terms  of  velocities. 

As  can  be  recognized  from  (3.123),  the  shape  and  orientation  of  the  ellip¬ 
soid  are  determined  by  the  core  of  its  quadratic  form  and  then  by  the  matrix 
J  JT  which  is  in  general  a  function  of  the  manipulator  configuration.  The 
directions  of  the  principal  axes  of  the  ellipsoid  are  determined  by  the  eigen¬ 
vectors  Ui ,  for  i  =  1, . . . ,  r,  of  the  matrix  J  JT ,  while  the  dimensions  of  the 

axes  are  given  by  the  singular  values  of  J,  cq  =  \J Xi(J JT)1  for  i  =  1, . . . ,  r, 

where  A ,(JJT)  denotes  the  generic  eigenvalue  of  JJT. 

A  global  representative  measure  of  manipulation  ability  can  be  obtained 
by  considering  the  volume  of  the  ellipsoid.  This  volume  is  proportional  to  the 
quantity 

w(q)  =  \J  det(  J  (q)JT(q)) 

which  is  the  manipulability  measure  already  introduced  in  (3.56).  In  the  case 
of  a  nonredundant  manipulator  (r  =  n),  w  reduces  to 


w(q)  =  |det(J(<?))|  . 


(3.124) 


154 


3  Differential  Kinematics  and  Statics 


l 

0.5 

1  0 
-0.5 
-1 

0  0.5  1  1.5  2 

[m] 

Fig.  3.23.  Velocity  manipulability  ellipses  for  a  two-link  planar  arm  in  different 
postures 

It  is  easy  to  recognize  that  it  is  always  w  >  0,  except  for  a  manipulator  at  a 
singular  configuration  when  iu  =  0.  For  this  reason,  this  measure  is  usually 
adopted  as  a  distance  of  the  manipulator  from  singular  configurations. 


Example  3.6 

Consider  the  two-link  planar  arm.  From  the  expression  in  (3.41),  the  manipulability 
measure  is  in  this  case 

w  =  |det(J)|  =  aia2|s2|. 

Therefore,  as  a  function  of  the  arm  postures,  the  manipulability  is  maximum  for 
$2  =  ±7r/2.  On  the  other  hand,  for  a  given  constant  reach  ai  +  a2,  the  structure 
offering  the  maximum  manipulability,  independently  of  $1  and  $2,  is  the  one  with 
a\  =  a  2- 

These  results  have  a  biomimetic  interpretation  in  the  human  arm,  if  that  is 
regarded  as  a  two-link  arm  (arm  +  forearm).  The  condition  m  =  a 2  is  satisfied  with 
good  approximation.  Further,  the  elbow  angle  $2  is  usually  in  the  neighbourhood  of 
7t/2  in  the  execution  of  several  tasks,  such  as  that  of  writing.  Hence,  the  human  being 
tends  to  dispose  the  arm  in  the  most  dexterous  configuration  from  a  manipulability 
viewpoint. 

Figure  3.23  illustrates  the  velocity  manipulability  ellipses  for  a  certain  number  of 
postures  with  the  tip  along  the  horizontal  axis  and  01  =  02  =  1.  It  can  be  seen  that 
when  the  arm  is  outstretched  the  ellipsoid  is  very  thin  along  the  vertical  direction. 
Hence,  one  recovers  the  result  anticipated  in  the  study  of  singularities  that  the  arm 
in  this  posture  can  generate  tip  velocities  preferably  along  the  vertical  direction.  In 
Fig.  3.24,  moreover,  the  behaviour  of  the  minimum  and  maximum  singular  values  of 
the  matrix  J  is  illustrated  as  a  function  of  tip  position  along  axis  x\  it  can  be  verified 
that  the  minimum  singular  value  is  null  when  the  manipulator  is  at  a  singularity 
(retracted  or  outstretched). 

Therefore,  with  reference  to  the  postures,  manipulability  has  a  maximum  for 
$2  =  ±7r/2.  On  the  other  hand,  for  a  given  total  extension  ai  +  (12,  the  structure 
which,  independently  of  i9i  and  $2,  offers  the  largest  manipulability  is  that  with 


a  1  =02- 
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Fig.  3.24.  Minimum  and  maximum  singular  values  of  J  for  a  two-link  planar  arm 
as  a  function  of  the  arm  posture 


[m] 

Fig.  3.25.  Force  manipulability  ellipses  for  a  two-link  planar  arm  in  different  pos¬ 
tures 


The  manipulability  measure  w  has  the  advantage  of  being  easy  to  compute, 
through  the  determinant  of  matrix  JJT.  However,  its  numerical  value  does 
not  constitute  an  absolute  measure  of  the  actual  closeness  of  the  manipulator 
to  a  singularity.  It  is  enough  to  consider  the  above  example  and  take  two 
arms  of  identical  structure,  one  with  links  of  1  m  and  the  other  with  links  of 
1  cm.  Two  different  values  of  manipulability  are  obtained  which  differ  by  four 
orders  of  magnitude.  Hence,  in  that  case  it  is  convenient  to  consider  only  |s2 1 
eventually  1I2I  -  as  the  manipulability  measure.  In  more  general  cases 
when  it  is  not  easy  to  find  a  simple,  meaningful  index,  one  can  consider  the 
ratio  between  the  minimum  and  maximum  singular  values  of  the  Jacobian 
<jr/ a\  which  is  equivalent  to  the  inverse  of  the  condition  number  of  matrix  J . 
This  ratio  gives  not  only  a  measure  of  the  distance  from  a  singularity  ( ay  =  0), 
but  also  a  direct  measure  of  eccentricity  of  the  ellipsoid.  The  disadvantage  in 
utilizing  this  index  is  its  computational  complexity;  it  is  practically  impossible 
to  compute  it  in  symbolic  form,  i.e.,  as  a  function  of  the  joint  configuration, 
except  for  matrices  of  reduced  dimension. 

On  the  basis  of  the  existing  duality  between  differential  kinematics  and 
statics,  it  is  possible  to  describe  the  manipulability  of  a  structure  not  only 
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with  reference  to  velocities,  but  also  with  reference  to  forces.  To  be  specific, 
one  can  consider  the  sphere  in  the  space  of  joint  torques 

ttt  =  1  (3.125) 

which,  accounting  for  (3.111),  is  mapped  into  the  ellipsoid  in  the  space  of 
end-effector  forces 

7e  ( J(q)JT(q)he  =  1  (3.126) 

which  is  defined  as  the  force  manipulability  ellipsoid.  This  ellipsoid  character¬ 
izes  the  end-effector  forces  that  can  be  generated  with  the  given  set  of  joint 
torques,  with  the  manipulator  in  a  given  posture. 

As  can  be  easily  recognized  from  (3.126),  the  core  of  the  quadratic  form  is 
constituted  by  the  inverse  of  the  matrix  core  of  the  velocity  ellipsoid  in  (3.123). 
This  feature  leads  to  the  notable  result  that  the  principal  axes  of  the  force 
manipulability  ellipsoid  coincide  with  the  principal  axes  of  the  velocity  manip¬ 
ulability  ellipsoid,  while  the  dimensions  of  the  respective  axes  are  in  inverse 
proportion.  Therefore,  according  to  the  concept  of  force/ velocity  duality,  a 
direction  along  which  good  velocity  manipulability  is  obtained  is  a  direction 
along  which  poor  force  manipulability  is  obtained,  and  vice  versa. 

In  Fig.  3.25,  the  manipulability  ellipses  for  the  same  postures  as  those 
of  the  example  in  Fig.  3.23  are  illustrated.  A  comparison  of  the  shape  and 
orientation  of  the  ellipses  confirms  the  force/ velocity  duality  effect  on  the 
manipulability  along  different  directions. 

It  is  worth  pointing  out  that  these  manipulability  ellipsoids  can  be  repre¬ 
sented  geometrically  in  all  cases  of  an  operational  space  of  dimension  at  most 
3.  Therefore,  if  it  is  desired  to  analyze  manipulability  in  a  space  of  greater 
dimension,  it  is  worth  separating  the  components  of  linear  velocity  (force) 
from  those  of  angular  velocity  (moment),  also  avoiding  problems  due  to  non- 
homogeneous  dimensions  of  the  relevant  quantities  (e.g.,  m/s  vs  rad/s).  For 
instance,  for  a  manipulator  with  a  spherical  wrist,  the  manipulability  analysis 
is  naturally  prone  to  a  decoupling  between  arm  and  wrist. 

An  effective  interpretation  of  the  above  results  can  be  achieved  by  regard¬ 
ing  the  manipulator  as  a  mechanical  transformer  of  velocities  and  forces  from 
the  joint  space  to  the  operational  space.  Conservation  of  energy  dictates  that 
an  amplification  in  the  velocity  transformation  is  necessarily  accompanied  by 
a  reduction  in  the  force  transformation,  and  vice  versa.  The  transformation 
ratio  along  a  given  direction  is  determined  by  the  intersection  of  the  vector 
along  that  direction  with  the  surface  of  the  ellipsoid.  Once  a  unit  vector  u 
along  a  direction  has  been  assigned,  it  is  possible  to  compute  the  transforma¬ 
tion  ratio  for  the  force  manipulability  ellipsoid  as 


a(q) 


(uT  J(q)JT{q)uj 


-1/2 


(3.127) 
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Fig.  3.26.  Velocity  and  force  manipulability  ellipses  for  a  3-link  planar  arm  in  a 
typical  configuration  for  a  task  of  controlling  force  and  velocity 


and  for  the  velocity  manipulability  ellipsoid  as 

P(q)  =  (uT  {J{q)JT{q))  ^  ■  (3.128) 

The  manipulability  ellipsoids  can  be  conveniently  utilized  not  only  for  an¬ 
alyzing  manipulability  of  the  structure  along  different  directions  of  the  opera¬ 
tional  space,  but  also  for  determining  compatibility  of  the  structure  to  execute 
a  task  assigned  along  a  direction.  To  this  end,  it  is  useful  to  distinguish  be¬ 
tween  actuation  tasks  and  control  tasks  of  velocity  and  force.  In  terms  of  the 
relative  ellipsoid,  the  task  of  actuating  a  velocity  (force)  requires  preferably 
a  large  transformation  ratio  along  the  task  direction,  since  for  a  given  set  of 
joint  velocities  (forces)  at  the  joints  it  is  possible  to  generate  a  large  velocity 
(force)  at  the  end-effector.  On  the  other  hand,  for  a  control  task  it  is  impor¬ 
tant  to  have  a  small  transformation  ratio  so  as  to  gain  good  sensitivity  to 
errors  that  may  occur  along  the  given  direction. 

Revisiting  once  again  the  duality  between  velocity  manipulability  ellipsoid 
and  force  manipulability  ellipsoid,  it  can  be  found  that  an  optimal  direction  to 
actuate  a  velocity  is  also  an  optimal  direction  to  control  a  force.  Analogously, 
a  good  direction  to  actuate  a  force  is  also  a  good  direction  to  control  a  velocity. 

To  have  a  tangible  example  of  the  above  concept,  consider  the  typical  task 
of  writing  on  a  horizontal  surface  for  the  human  arm;  this  time,  the  arm  is  re¬ 
garded  as  a  3-link  planar  arm:  arm  +  forearm  +  hand.  Restricting  the  analysis 
to  a  two-dimensional  task  space  (the  direction  vertical  to  the  surface  and  the 
direction  of  the  line  of  writing),  one  has  to  achieve  fine  control  of  the  vertical 
force  (the  pressure  of  the  pen  on  the  paper)  and  of  the  horizontal  velocity  (to 
write  in  good  calligraphy) .  As  a  consequence,  the  force  manipulability  ellipse 
tends  to  be  oriented  horizontally  for  correct  task  execution.  Correspondingly, 
the  velocity  manipulability  ellipse  tends  to  be  oriented  vertically  in  perfect 
agreement  with  the  task  requirement.  In  this  case,  from  Fig.  3.26  the  typical 
configuration  of  the  human  arm  when  writing  can  be  recognized. 
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Fig.  3.27.  Velocity  and  force  manipulability  ellipses  for  a  3-link  planar  arm  in  a 
typical  configuration  for  a  task  of  actuating  force  and  velocity 


An  opposite  example  to  the  previous  one  is  that  of  the  human  arm  when 
throwing  a  weight  in  the  horizontal  direction.  In  fact,  now  it  is  necessary  to 
actuate  a  large  vertical  force  (to  sustain  the  weight)  and  a  large  horizontal 
velocity  (to  throw  the  load  for  a  considerable  distance).  Unlike  the  above,  the 
force  (velocity)  manipulability  ellipse  tends  to  be  oriented  vertically  (horizon¬ 
tally)  to  successfully  execute  the  task.  The  relative  configuration  in  Fig.  3.27 
is  representative  of  the  typical  attitude  of  the  human  arm  when,  for  instance, 
releasing  the  ball  in  a  bowling  game. 

In  the  above  two  examples,  it  is  worth  pointing  out  that  the  presence  of  a 
two-dimensional  operational  space  is  certainly  advantageous  to  try  reconfig¬ 
uring  the  structure  in  the  best  configuration  compatible  with  the  given  task. 
In  fact,  the  transformation  ratios  defined  in  (3.127)  and  (3.128)  are  scalar 
functions  of  the  manipulator  configurations  that  can  be  optimized  locally  ac¬ 
cording  to  the  technique  for  exploiting  redundant  DOFs  previously  illustrated. 


Bibliography 

The  concept  of  geometric  Jacobian  was  originally  introduced  in  [240]  and  the 
problem  of  its  computationally  efficient  determination  is  considered  in  [173]. 
The  concept  of  analytical  Jacobian  is  presented  in  [114]  with  reference  to 
operational  space  control. 

Inverse  differential  kinematics  dates  back  to  [240]  under  the  name  of  re¬ 
solved  rate  control.  The  use  of  the  Jacobian  pseudo-inverse  is  due  to  [118]. 
The  adoption  of  the  damped  least-squares  inverse  has  been  independently 
proposed  by  [161]  and  [238];  a  tutorial  on  the  topic  is  [42].  The  inverse 
kinematics  algorithm  based  on  the  Jacobian  transpose  has  been  originally 
proposed  in  [198,  16].  Further  details  about  the  orientation  error  are  found 
in  [142,  250,  132,  41]. 

The  utilization  of  the  joint  velocities  in  the  null  space  of  the  Jacobian  for 
redundancy  resolution  is  proposed  in  [129]  and  further  refined  in  [147]  regard¬ 
ing  the  choice  of  the  objective  functions.  The  approach  based  on  task  priority 


Problems  159 


is  presented  in  [163];  other  approaches  based  on  the  concept  of  augmented 
task  space  are  presented  in  [14,  69,  199,  203,  194,  37].  For  global  redundancy 
resolutions  see  [162].  A  complete  treatment  of  redundant  manipulators  can  be 
found  in  [160]  while  a  tutorial  is  [206]. 

The  extension  of  inverse  kinematics  to  the  second  order  has  been  proposed 
in  [207] ,  while  the  symbolic  differentiation  of  the  solutions  in  terms  of  joint 
velocities  to  obtain  stable  acceleration  solutions  can  be  found  in  [208].  Further 
details  about  redundancy  resolution  are  in  [59]. 

The  concepts  of  kineto-statics  duality  are  discussed  in  [191].  The  manipu- 
lability  ellipsoids  are  proposed  in  [245,  248]  and  employed  in  [44]  for  posture 
dexterity  analysis  with  regard  to  manipulation  tasks. 


Problems 

3.1.  Prove  (3.11). 

3.2.  Compute  the  Jacobian  of  the  cylindrical  arm  in  Fig.  2.35. 

3.3.  Compute  the  Jacobian  of  the  SCARA  manipulator  in  Fig.  2.36. 

3.4.  Find  the  singularities  of  the  3-link  planar  arm  in  Fig.  2.20. 

3.5.  Find  the  singularities  of  the  spherical  arm  in  Fig.  2.22. 

3.6.  Find  the  singularities  of  the  cylindrical  arm  in  Fig.  2.35. 

3.7.  Find  the  singularities  of  the  SCARA  manipulator  in  Fig.  2.36. 

3.8.  Show  that  the  manipulability  measure  defined  in  (3.56)  is  given  by  the 
product  of  the  singular  values  of  the  Jacobian  matrix. 

3.9.  For  the  3-link  planar  arm  in  Fig.  2.20,  find  an  expression  of  the  distance 
of  the  arm  from  a  circular  obstacle  of  given  radius  and  coordinates. 

3.10.  Find  the  solution  to  the  differential  kinematics  equation  with  the 
damped  least-square  inverse  in  (3.59). 

3.11.  Prove  (3.64)  in  an  alternative  way,  i.e. ,  by  computing  S(u>e)  as  in  (3.6) 
starting  from  R((f>)  in  (2.18). 

3.12.  With  reference  to  (3.64),  find  the  transformation  matrix  T(4>e)  in  the 
case  of  RPY  angles. 

3.13.  With  reference  to  (3.64),  find  the  triplet  of  Euler  angles  for  which 

T(0)  =  I. 


3.14.  Show  how  the  inverse  kinematics  scheme  of  Fig.  3.11  can  be  simplified 
in  the  case  of  a  manipulator  having  a  spherical  wrist. 
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3.15.  Find  an  expression  of  the  upper  bound  on  the  norm  of  e  for  the  solu¬ 
tion  (3.76)  in  the  case  xd  ^  0. 

3.16.  Prove  (3.85). 

3.17.  Prove  (3.86),  (3.87). 

3.18.  Prove  that  the  equation  relating  the  angular  velocity  to  the  time  deriva¬ 
tive  of  the  quaternion  is  given  by 

uj  =  2  S(e)e  +  2r)e  —  2rje. 

[Hint:  Start  by  showing  that  (2.33)  can  be  rewritten  as  R(rj,  e)  =  (2if  — 1)1  + 
2eeT  +  2r]S(e)]. 

3.19.  Prove  (3.94),  (3.95). 

3.20.  Prove  that  the  time  derivative  of  the  Lyapunov  function  in  (3.96)  is 
given  by  (3.97). 

3.21.  Consider  the  3-link  planar  arm  in  Fig.  2.20,  whose  link  lengths  are 

respectively  0.5  m,  0.3  m,  0.3  m.  Perform  a  computer  implementation  of  the 
inverse  kinematics  algorithm  using  the  Jacobian  pseudo-inverse  along  the  op¬ 
erational  space  path  given  by  a  straight  line  connecting  the  points  of  coordi¬ 
nates  (0.8,  0.2)  m  and  (0.8,  —0.2)  m.  Add  a  constraint  aimed  at  avoiding  link 
collision  with  a  circular  object  located  at  0  =  [0.3  0  ]T  m  of  radius  0.1m.  The 

initial  arm  configuration  is  chosen  so  that  pe( 0)  =  pd(0).  The  final  time  is 
2  s.  Use  sinusoidal  motion  timing  laws.  Adopt  the  Euler  numerical  integration 
scheme  (3.48)  with  an  integration  time  At  =  1  ms. 

3.22.  Consider  the  SCARA  manipulator  in  Fig.  2.36,  whose  links  both  have  a 
length  of  0.5  m  and  are  located  at  a  height  of  lm  from  the  supporting  plane. 
Perform  a  computer  implementation  of  the  inverse  kinematics  algorithms  with 
both  Jacobian  inverse  and  Jacobian  transpose  along  the  operational  space 
path  whose  position  is  given  by  a  straight  line  connecting  the  points  of  co¬ 
ordinates  (0.7,  0,0)  m  and  (0, 0.8,  0.5)  m,  and  whose  orientation  is  given  by 
a  rotation  from  Orad  to  7r/2rad.  The  initial  arm  configuration  is  chosen  so 
that  £ce(0)  =  £Cd(0).  The  final  time  is  2s.  Use  sinusoidal  motion  timing  laws. 
Adopt  the  Euler  numerical  integration  scheme  (3.48)  with  an  integration  time 
At  =  1  ms. 

3.23.  Prove  that  the  directions  of  the  principal  axes  of  the  force  and  velocity 
manipulability  ellipsoids  coincide  while  their  dimensions  are  in  inverse  pro¬ 
portion. 
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Trajectory  Planning 


For  the  execution  of  a  specific  robot  task,  it  is  worth  considering  the  main 
features  of  motion  planning  algorithms.  The  goal  of  trajectory  planning  is  to 
generate  the  reference  inputs  to  the  motion  control  system  which  ensures  that 
the  manipulator  executes  the  planned  trajectories.  The  user  typically  specifies 
a  number  of  parameters  to  describe  the  desired  trajectory.  Planning  consists  of 
generating  a  time  sequence  of  the  values  attained  by  an  interpolating  function 
(typically  a  polynomial)  of  the  desired  trajectory.  This  chapter  presents  some 
techniques  for  trajectory  generation,  both  in  the  case  when  the  initial  and 
final  point  of  the  path  are  assigned  ( point-to-point  motion ),  and  in  the  case 
when  a  finite  sequence  of  points  are  assigned  along  the  path  ( motion  through 
a  sequence  of  points).  First,  the  problem  of  trajectory  planning  in  the  joint 
space  is  considered,  and  then  the  basic  concepts  of  trajectory  planning  in 
the  operational  space  are  illustrated.  The  treatment  of  the  motion  planning 
problem  for  mobile  robots  is  deferred  to  Chap.  12. 


4.1  Path  and  Trajectory 

The  minimal  requirement  for  a  manipulator  is  the  capability  to  move  from 
an  initial  posture  to  a  final  assigned  posture.  The  transition  should  be  char¬ 
acterized  by  motion  laws  requiring  the  actuators  to  exert  joint  generalized 
forces  which  do  not  violate  the  saturation  limits  and  do  not  excite  the  typi¬ 
cally  modelled  resonant  modes  of  the  structure.  It  is  then  necessary  to  devise 
planning  algorithms  that  generate  suitably  smooth  trajectories. 

In  order  to  avoid  confusion  between  terms  often  used  as  synonyms,  the 
difference  between  a  path  and  a  trajectory  is  to  be  explained.  A  path  denotes 
the  locus  of  points  in  the  joint  space,  or  in  the  operational  space,  which  the 
manipulator  has  to  follow  in  the  execution  of  the  assigned  motion;  a  path  is 
then  a  pure  geometric  description  of  motion.  On  the  other  hand,  a  trajectory 
is  a  path  on  which  a  timing  law  is  specified,  for  instance  in  terms  of  velocities 
and/or  accelerations  at  each  point. 
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In  principle,  it  can  be  conceived  that  the  inputs  to  a  trajectory  planning 
algorithm  are  the  path  description,  the  path  constraints,  and  the  constraints 
imposed  by  manipulator  dynamics,  whereas  the  outputs  are  the  end-effector 
trajectories  in  terms  of  a  time  sequence  of  the  values  attained  by  position, 
velocity  and  acceleration. 

A  geometric  path  cannot  be  fully  specified  by  the  user  for  obvious  com¬ 
plexity  reasons.  Typically,  a  reduced  number  of  parameters  is  specified  such 
as  extremal  points,  possible  intermediate  points,  and  geometric  primitives  in¬ 
terpolating  the  points.  Also,  the  motion  timing  law  is  not  typically  specified 
at  each  point  of  the  geometric  path,  but  rather  it  regards  the  total  trajectory 
time,  the  constraints  on  the  maximum  velocities  and  accelerations,  and  even¬ 
tually  the  assignment  of  velocity  and  acceleration  at  points  of  particular  inter¬ 
est.  On  the  basis  of  the  above  information,  the  trajectory  planning  algorithm 
generates  a  time  sequence  of  variables  that  describe  end-effector  position  and 
orientation  over  time  in  respect  of  the  imposed  constraints.  Since  the  control 
action  on  the  manipulator  is  carried  out  in  the  joint  space,  a  suitable  inverse 
kinematics  algorithm  is  to  be  used  to  reconstruct  the  time  sequence  of  joint 
variables  corresponding  to  the  above  sequence  in  the  operational  space. 

Trajectory  planning  in  the  operational  space  naturally  allows  the  presence 
of  path  constraints  to  be  accounted;  these  are  due  to  regions  of  workspace 
which  are  forbidden  to  the  manipulator,  e.g.,  due  to  the  presence  of  obstacles. 
In  fact,  such  constraints  are  typically  better  described  in  the  operational  space, 
since  their  corresponding  points  in  the  joint  space  are  difficult  to  compute. 

With  regard  to  motion  in  the  neighbourhood  of  singular  configurations  and 
presence  of  redundant  DOFs,  trajectory  planning  in  the  operational  space  may 
involve  problems  difficult  to  solve.  In  such  cases,  it  may  be  advisable  to  specify 
the  path  in  the  joint  space,  still  in  terms  of  a  reduced  number  of  parameters. 
Hence,  a  time  sequence  of  joint  variables  has  to  be  generated  which  satisfy 
the  constraints  imposed  on  the  trajectory. 

For  the  sake  of  clarity,  in  the  following,  the  case  of  joint  space  trajectory 
planning  is  treated  first.  The  results  will  then  be  extended  to  the  case  of 
trajectories  in  the  operational  space. 


4.2  Joint  Space  Trajectories 

A  manipulator  motion  is  typically  assigned  in  the  operational  space  in  terms 
of  trajectory  parameters  such  as  the  initial  and  final  end-effector  pose,  possi¬ 
ble  intermediate  poses,  and  travelling  time  along  particular  geometric  paths. 
If  it  is  desired  to  plan  a  trajectory  in  the  joint  space,  the  values  of  the  joint 
variables  have  to  be  determined  first  from  the  end-effector  position  and  ori¬ 
entation  specified  by  the  user.  It  is  then  necessary  to  resort  to  an  inverse 
kinematics  algorithm,  if  planning  is  done  off-line,  or  to  directly  measure  the 
above  variables,  if  planning  is  done  by  the  teaching-by-showing  technique  (see 
Chap.  6). 
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The  planning  algorithm  generates  a  function  q(t )  interpolating  the  given 
vectors  of  joint  variables  at  each  point,  in  respect  of  the  imposed  constraints. 

In  general,  a  joint  space  trajectory  planning  algorithm  is  required  to  have 
the  following  features: 

•  the  generated  trajectories  should  be  not  very  demanding  from  a  compu¬ 
tational  viewpoint, 

•  the  joint  positions  and  velocities  should  be  continuous  functions  of  time 
(continuity  of  accelerations  may  be  imposed,  too), 

•  undesirable  effects  should  be  minimized,  e.g.,  nonsmooth  trajectories  in¬ 
terpolating  a  sequence  of  points  on  a  path. 

At  first,  the  case  is  examined  when  only  the  initial  and  final  points  on 
the  path  and  the  traveling  time  are  specified  ( point-to-point );  the  results  are 
then  generalized  to  the  case  when  also  intermediate  points  along  the  path  are 
specified  ( motion  through  a  sequence  of  points).  Without  loss  of  generality, 
the  single  joint  variable  q{t )  is  considered. 

4.2.1  Point-to-Point  Motion 

In  point-to-point  motion,  the  manipulator  has  to  move  from  an  initial  to  a 
final  joint  configuration  in  a  given  time  tf.  In  this  case,  the  actual  end-effector 
path  is  of  no  concern.  The  algorithm  should  generate  a  trajectory  which,  in 
respect  to  the  above  general  requirements,  is  also  capable  of  optimizing  some 
performance  index  when  the  joint  is  moved  from  one  position  to  another. 

A  suggestion  for  choosing  the  motion  primitive  may  stem  from  the  analysis 
of  an  incremental  motion  problem.  Let  I  be  the  moment  of  inertia  of  a  rigid 
body  about  its  rotation  axis.  It  is  required  to  take  the  angle  q  from  an  initial 
value  qt  to  a  final  value  qf  in  a  time  tf.  It  is  obvious  that  infinite  solutions 
exist  to  this  problem.  Assumed  that  rotation  is  executed  through  a  torque  r 
supplied  by  a  motor,  a  solution  can  be  found  which  minimizes  the  energy  dis¬ 
sipated  in  the  motor.  This  optimization  problem  can  be  formalized  as  follows. 
Having  set  q  =  to,  determine  the  solution  to  the  differential  equation 

Id)  =  T 


subject  to  the  condition 

f 

Lu{t)dt  =  qp  -  qi 
so  as  to  minimize  the  performance  index 

r2(t)dt. 

It  can  be  shown  that  the  resulting  solution  is  of  the  type 

co(t)  =  at 2  +  bt  +  c. 
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Even  though  the  joint  dynamics  cannot  be  described  in  the  above  simple 
manner,1  the  choice  of  a  third-order  polynomial  function  to  generate  a  joint 
trajectory  represents  a  valid  solution  for  the  problem  at  issue. 

Therefore,  to  determine  a  joint  motion,  the  cubic  polynomial 

q(f)  =  03 t3  +  a2t2  +  flit  +  do  (4-1) 

can  be  chosen,  resulting  into  a  parabolic  velocity  profile 

q(t)  =  3a3 12  +  2  a2f  +  ai 
and  a  linear  acceleration  profile 

q(t)  =  6  a3t  +  2  a2. 

Since  four  coefficients  are  available,  it  is  possible  to  impose,  besides  the  initial 
and  final  joint  position  values  qi  and  qf ,  also  the  initial  and  final  joint  velocity 
values  qi  and  qf  which  are  usually  set  to  zero.  Determination  of  a  specific 
trajectory  is  given  by  the  solution  to  the  following  system  of  equations: 


ao  =  qi 
fli  =  qi 

o.3t  f  +  a2tf  +  fli  tf  +  ao  =  qf 
3a3t2  +  2ci2  tf  +  ai  =  qf, 

that  allows  the  computation  of  the  coefficients  of  the  polynomial  in  (4.1). 2 
Figure  4.1  illustrates  the  timing  law  obtained  with  the  following  data:  qi  =  0, 
qf  =  7r,  tf  =  1,  and  qi  =  qf  —  0.  As  anticipated,  velocity  has  a  parabolic  pro¬ 
file,  while  acceleration  has  a  linear  profile  with  initial  and  final  discontinuity. 

If  it  is  desired  to  assign  also  the  initial  and  final  values  of  acceleration,  six 
constraints  have  to  be  satisfied  and  then  a  polynomial  of  at  least  fifth,  order 
is  needed.  The  motion  timing  law  for  the  generic  joint  is  then  given  by 

q{t)  =  a5t5  +  +  a3t3  +  a2t2  +  a\ t  +  a0,  (4.2) 

whose  coefficients  can  be  computed,  as  for  the  previous  case,  by  imposing  the 
conditions  for  t,  =  0  and  t  =  tf  on  the  joint  variable  q(t)  and  on  its  first 
two  derivatives.  With  the  choice  (4.2),  one  obviously  gives  up  minimizing  the 
above  performance  index. 

An  alternative  approach  with  timing  laws  of  blended  polynomial  type  is 
frequently  adopted  in  industrial  practice,  which  allows  a  direct  verification 


1  In  fact,  recall  that  the  moment  of  inertia  about  the  joint  axis  is  a  function  of 
manipulator  configuration. 

2  Notice  that  it  is  possible  to  normalize  the  computation  of  the  coefficients,  so  as 

to  be  independent  both  on  the  final  time  tf  and  on  the  path  length  \qj  —  qt\. 
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Fig.  4.1.  Time  history  of  position,  velocity  and  acceleration  with  a  cubic  polynomial 
timing  law 


of  whether  the  resulting  velocities  and  accelerations  can  be  supported  by  the 
physical  mechanical  manipulator. 

In  this  case,  a  trapezoidal  velocity  profile  is  assigned,  which  imposes  a 
constant  acceleration  in  the  start  phase,  a  cruise  velocity,  and  a  constant 
deceleration  in  the  arrival  phase.  The  resulting  trajectory  is  formed  by  a  linear 
segment  connected  by  two  parabolic  segments  to  the  initial  and  final  positions. 

In  the  following,  the  problem  is  formulated  by  assuming  that  the  final  time 
of  trajectory  duration  has  been  assigned.  However,  in  industrial  practice,  the 
user  is  offered  the  option  to  specify  the  velocity  percentage  with  respect  to  the 
maximum  allowable  velocity;  this  choice  is  aimed  at  avoiding  occurrences  when 
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Fig.  4.2.  Characterization  of  a  timing  law  with  trapezoidal  velocity  profile  in  terms 
of  position,  velocity  and  acceleration 


the  specification  of  a  much  too  short  motion  duration  would  involve  much  too 
large  values  of  velocities  and/or  accelerations,  beyond  those  achievable  by  the 
manipulator. 

As  can  be  seen  from  the  velocity  profiles  in  Fig.  4.2,  it  is  assumed  that  both 
initial  and  final  velocities  are  null  and  the  segments  with  constant  accelerations 
have  the  same  time  duration;  this  implies  an  equal  magnitude  qc  in  the  two 
segments.  Notice  also  that  the  above  choice  leads  to  a  symmetric  trajectory 
with  respect  to  the  average  point  qm  =  ( qf  +  qi)/2  at  tm  =  tf/ 2. 

The  trajectory  has  to  satisfy  some  constraints  to  ensure  the  transition 
from  qi  to  qj  in  a  time  tf.  The  velocity  at  the  end  of  the  parabolic  segment 
must  be  equal  to  the  (constant)  velocity  of  the  linear  segment,  i.e., 


qctc 


Qm  Qc 
tm  to 


(4.3) 


where  qc  is  the  value  attained  by  the  joint  variable  at  the  end  of  the  parabolic 
segment  at  time  tc  with  constant  acceleration  qc  (recall  that  g(0)  =  0).  It  is 
then 


Qc  =  qi  +  2?ctc- 


(4.4) 


Combining  (4.3),  (4.4)  gives 


qctl  -  qctftc  +  qf  -  qt  =  0. 


(4.5) 
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Usually,  qc  is  specified  with  the  constraint  that  sgngc  =  sgn  (qf  —  q%):  hence, 
for  given  tf,  qi  and  qf,  the  solution  for  tc  is  computed  from  (4.5)  as  (tc  <  tf / 2) 


+  _  tf  1  ./*/&-  4(g/  -  qi) 
c  ^  2  2  V  Qc 

Acceleration  is  then  subject  to  the  constraint 

4I Qf  ~  Qi\ 


\Qc\  > 


(4.6) 


(4.7) 


When  the  acceleration  qc  is  chosen  so  as  to  satisfy  (4.7)  with  the  equality 
sign,  the  resulting  trajectory  does  not  feature  the  constant  velocity  segment 
any  more  and  has  only  the  acceleration  and  deceleration  segments  ( triangular 
profile). 

Given  qt ,  qf  and  tf,  and  thus  also  an  average  transition  velocity,  the  con¬ 
straint  in  (4.7)  allows  the  imposition  of  a  value  of  acceleration  consistent  with 
the  trajectory.  Then,  tc  is  computed  from  (4.6),  and  the  following  sequence  of 
polynomials  is  generated: 

[qi+  \qct2  0  <t<tc 

q(t)  =  <  qi  +  qctc(t  -  tc/ 2)  tc  <  t  <  tf  -  tc  (4.8) 

l  Qf  -  -t)2  tf-tc<t<  tf. 

Figure  4.3  illustrates  a  representation  of  the  motion  timing  law  obtained  by 
imposing  the  data:  q%  =  0 ,  qf  =  7r,  tf  =  1,  and  \qc\  =  67r. 

Specifying  acceleration  in  the  parabolic  segment  is  not  the  only  way  to 
determine  trajectories  with  trapezoidal  velocity  profile.  Besides  qi,  qf  and  tf, 
one  can  specify  also  the  cruise  velocity  qc  which  is  subject  to  the  constraint 

tf  tf 

By  recognizing  that  qc  =  qctc,  (4-5)  allows  the  computation  of  tc  as 


*  _  qt-  qf  +  qctf 

— 

qc 

and  thus  the  resulting  acceleration  is 


qi  -  qf  +  Qctf  ' 


(4.10) 


(4.11) 


The  computed  values  of  tc  and  qc  as  in  (4.10),  (4.11)  allow  the  generation  of 
the  sequence  of  polynomials  expressed  by  (4.8). 

The  adoption  of  a  trapezoidal  velocity  profile  results  in  a  worse  perfor¬ 
mance  index  compared  to  the  cubic  polynomial.  The  decrease  is,  however, 
limited;  the  term  f^f  r2dt  increases  by  12.5%  with  respect  to  the  optimal 
case. 
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Fig.  4.3.  Time  history  of  position,  velocity  and  acceleration  with  a  trapezoidal 
velocity  profile  timing  law 


4.2.2  Motion  Through  a  Sequence  of  Points 

In  several  applications,  the  path  is  described  in  terms  of  a  number  of  points 
greater  than  two.  For  instance,  even  for  the  simple  point-to-point  motion 
of  a  pick-and-place  task,  it  may  be  worth  assigning  two  intermediate  points 
between  the  initial  point  and  the  final  point;  suitable  positions  can  be  set  for 
lifting  off  and  setting  down  the  object,  so  that  reduced  velocities  are  obtained 
with  respect  to  direct  transfer  of  the  object.  For  more  complex  applications, 
it  may  be  convenient  to  assign  a  sequence  of  points  so  as  to  guarantee  better 
monitoring  on  the  executed  trajectories;  the  points  are  to  be  specified  more 
densely  in  those  segments  of  the  path  where  obstacles  have  to  be  avoided 
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Fig.  4.4.  Characterization  of  a  trajectory  on  a  given  path  obtained  through  inter¬ 
polating  polynomials 


or  a  high  path  curvature  is  expected.  It  should  not  be  forgotten  that  the 
corresponding  joint  variables  have  to  be  computed  from  the  operational  space 
poses. 

Therefore,  the  problem  is  to  generate  a  trajectory  when  N  points,  termed 
path  points,  are  specified  and  have  to  be  reached  by  the  manipulator  at  certain 
instants  of  time.  For  each  joint  variable  there  are  N  constraints,  and  then  one 
might  want  to  use  an  (iV—  l)-order  polynomial.  This  choice,  however,  has  the 
following  disadvantages: 

•  It  is  not  possible  to  assign  the  initial  and  final  velocities. 

•  As  the  order  of  a  polynomial  increases,  its  oscillatory  behaviour  increases, 
and  this  may  lead  to  trajectories  which  are  not  natural  for  the  manipulator. 

•  Numerical  accuracy  for  computation  of  polynomial  coefficients  decreases 
as  order  increases. 

•  The  resulting  system  of  constraint  equations  is  heavy  to  solve. 

•  Polynomial  coefficients  depend  on  all  the  assigned  points;  thus,  if  it  is 
desired  to  change  a  point,  all  of  them  have  to  be  recomputed. 

These  drawbacks  can  be  overcome  if  a  suitable  number  of  low-order  inter¬ 
polating  polynomials,  continuous  at  the  path  points,  are  considered  in  place 
of  a  single  high-orcler  polynomial. 

According  to  the  previous  section,  the  interpolating  polynomial  of  lowest 
order  is  the  cubic  polynomial,  since  it  allows  the  imposition  of  continuity  of 
velocities  at  the  path  points.  With  reference  to  the  single  joint  variable,  a 
function  q(t)  is  sought,  formed  by  a  sequence  of  N  —  1  cubic  polynomials 
77fc(t),  for  k  =  1, . . . ,  N  —  1,  continuous  with  continuous  first  derivatives.  The 
function  q(t)  attains  the  values  qi-  for  t.  =  t k  (k  =  1  ,...,N),  and  qi  =  qt, 
ti  =  0,  qN  =  qf,  t n  =  t/\  the  qk  s  represent  the  path  points  describing 
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the  desired  trajectory  at  t  =  tk  (Fig.  4.4).  The  following  situations  can  be 
considered: 

•  Arbitrary  values  of  q(t)  are  imposed  at  the  path  points. 

•  The  values  of  q(t)  at  the  path  points  are  assigned  according  to  a  certain 
criterion. 

•  The  acceleration  q(t)  has  to  be  continuous  at  the  path  points. 

To  simplify  the  problem,  it  is  also  possible  to  find  interpolating  polynomials 
of  order  less  than  three  which  determine  trajectories  passing  nearby  the  path 
points  at  the  given  instants  of  time. 

Interpolating  polynomials  with  imposed  velocities  at  path  points 

This  solution  requires  the  user  to  be  able  to  specify  the  desired  velocity  at 
each  path  point;  the  solution  does  not  possess  any  novelty  with  respect  to  the 
above  concepts. 

The  system  of  equations  allowing  computation  of  the  coefficients  of  the 
N  —  1  cubic  polynomials  interpolating  the  N  path  points  is  obtained  by  im¬ 
posing  the  following  conditions  on  the  generic  polynomial  Ilk  (t)  interpolating 
qk  and  qk+ 1,  for  k  =  1, . . . ,  N  -  1: 

Fffc(tfe)  =  qk 

IIk{tk+ 1)  =  qk+ 1 
nk{tk)  =  qk 
Hh{tk+ 1)  =  Qk+1- 

The  result  is  N  —  1  systems  of  four  equations  in  the  four  unknown  coefficients 
of  the  generic  polynomial;  these  can  be  solved  one  independently  of  the  other. 
The  initial  and  final  velocities  of  the  trajectory  are  typically  set  to  zero  (<ji  = 
qN  =  0)  and  continuity  of  velocity  at  the  path  points  is  ensured  by  setting 

IIk{tk+ 1)  =  IIk+\(tk+l) 


for  k  =  1, . . . ,  N  —  2. 

Figure  4.5  illustrates  the  time  history  of  position,  velocity  and  acceleration 
obtained  with  the  data:  q±  =  0,  <72  =  27t,  <73  =  ir/2,  <74  =  7r,  t\  =  0,  tz  =  2,  £3  = 
3,  ti  =  5,  <71  =  0,  (72  =  7r,  <73  =  — 7r,  <74  =  0.  Notice  the  resulting  discontinuity 
on  the  acceleration,  since  only  continuity  of  velocity  is  guaranteed. 

Interpolating  polynomials  with  computed  velocities  at  path  points 

In  this  case,  the  joint  velocity  at  a  path  point  has  to  be  computed  according 
to  a  certain  criterion.  By  interpolating  the  path  points  with  linear  segments, 
the  relative  velocities  can  be  computed  according  to  the  following  rules: 

9i  =  0 
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Fig.  4.5.  Time  history  of  position,  velocity  and  acceleration  with  a  timing  law  of 
interpolating  polynomials  with  velocity  constraints  at  path  points 


•  _  /  0  sgn  (vk)  ^  Sgn  (ufe+i)  ,  . 

Qk  \|(«fc+«fc+i)  sgn  (ffc)  =  sgn  (ffe+i.) 

qN  =  0, 

where  Vk  =  (qk  ~  qk-i)/(tk  ~  tk- 1)  gives  the  slope  of  the  segment  in  the 
time  interval  [tk-i,tk\-  With  the  above  settings,  the  determination  of  the 
interpolating  polynomials  is  reduced  to  the  previous  case. 

Figure  4.6  illustrates  the  time  history  of  position,  velocity  and  acceleration 
obtained  with  the  following  data:  <71  =  0,  <72  =  27 r,  q%  =  n/2,  <74  =  7r,  t\  =  0, 
f 2  =  2,  t3  =  3,  t’4  =  5,  qi  =  0,  <74  =  0.  It  is  easy  to  recognize  that  the  imposed 


172 


4  Trajectory  Planning 


Fig.  4.6.  Time  history  of  position,  velocity  and  acceleration  with  a  timing  law  of 
interpolating  polynomials  with  computed  velocities  at  path  points 


sequence  of  path  points  leads  to  having  zero  velocity  at  the  intermediate 
points. 


Interpolating  polynomials  with  continuous  accelerations  at  path 
points  (splines) 

Both  the  above  two  solutions  do  not  ensure  continuity  of  accelerations  at 
the  path  points.  Given  a  sequence  of  N  path  points,  the  acceleration  is  also 
continuous  at  each  tk  if  four  constraints  are  imposed,  namely,  two  position 
constraints  for  each  of  the  adjacent  cubics  and  two  constraints  guaranteeing 
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continuity  of  velocity  and  acceleration.  The  following  equations  have  then  to 
be  satisfied: 


—  qk 

IIk-i{tk )  =  IIk{tk ) 

nk-i{tk)  =  ijk(tk) 

)  =  iik(tk)- 

The  resulting  system  for  the  N  path  points,  including  the  initial  and  final 
points,  cannot  be  solved.  In  fact,  it  is  formed  by  4(iV  —  2)  equations  for  the 
intermediate  points  and  6  equations  for  the  extremal  points;  the  position 
constraints  for  the  polynomials  I7o(ti)  =  qi  and  77jv(ty)  =  qf  have  to  be 
excluded  since  they  are  not  defined.  Also,  77o(ti),  ITo(ti),  iTjv(t/),  do 

not  have  to  be  counted  as  polynomials  since  they  are  just  the  imposed  values 
of  initial  and  final  velocities  and  accelerations.  In  summary,  one  has  4Ar  —  2 
equations  in  4(iV  —  1)  unknowns. 

The  system  can  be  solved  only  if  one  eliminates  the  two  equations  which 
allow  the  arbitrary  assignment  of  the  initial  and  final  acceleration  values. 
Fourth-order  polynomials  should  be  used  to  include  this  possibility  for  the 
first  and  last  segment. 

On  the  other  hand,  if  only  third-order  polynomials  are  to  be  used,  the  fol¬ 
lowing  deception  can  be  operated.  Two  virtual  points  are  introduced  for  which 
continuity  constraints  on  position,  velocity  and  acceleration  can  be  imposed, 
without  specifying  the  actual  positions,  though.  It  is  worth  remarking  that  the 
effective  location  of  these  points  is  irrelevant,  since  their  position  constraints 
regard  continuity  only.  Hence,  the  introduction  of  two  virtual  points  implies 
the  determination  of  N  +  1  cubic  polynomials. 

Consider  N  +  2  time  instants  tk,  where  t-2  and  t^+i  conventionally  refer  to 
the  virtual  points.  The  system  of  equations  for  determining  the  N  +  1  cubic 
polynomials  can  be  found  by  taking  the  4(7V  —  2)  equations: 


II 

1 

fc! 

(4.13) 

Tlfc-i(ffc)  =  Tlfc(tfe) 

(4.14) 

Tlfe-i(tfc)  =  Tlfc(tfe) 

(4.15) 

-to 

II 

HO 

7 

(4.16) 

for  k  =  3, . . . ,  N,  written  for  the  TV  —  2  intermediate  path  points,  the  6  equa¬ 
tions: 


TTi(ti)  =  qi 

(4.17) 

iliiti)  =  qi 

(4.18) 

ni{ti)  = 

(4.19) 

nN+l{tN+2)  =  9/ 

(4.20) 
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nN+i(tN+2)  =  9/  (4-21) 

nN+i(tN+2)  =  Qf  (4.22) 

written  for  the  initial  and  final  points,  and  the  6  equations: 

nk~i{tk)  =  nk(tk)  (4.23) 

nk-i(tk )  =  nk{tk)  (4.24) 

nk-i(tk)  =  flk{tk)  (4-25) 


for  k  =  2,  N  +  1,  written  for  the  two  virtual  points.  The  resulting  system 
has  4(7V  +  1)  equations  in  4(7V  +  1)  unknowns,  that  are  the  coefficients  of  the 
N  +  1  cubic  polynomials. 

The  solution  to  the  system  is  computationally  demanding,  even  for  low 
values  of  N.  Nonetheless,  the  problem  can  be  cast  in  a  suitable  form  so  as 
to  solve  the  resulting  system  of  equations  with  a  computationally  efficient 
algorithm.  Since  the  generic  polynomial  IIk{t)  is  a  cubic,  its  second  derivative 
must  be  a  linear  function  of  time  which  then  can  be  written  as 

nk(t)  =  -t)+  ^fc+l)  (t  -  tk)  k  =  l,...,N  +  l,  (4.26) 

Zltfc  /life 

where  Atk  =  tk+ 1  —  tk  indicates  the  time  interval  to  reach  qk+\  from  qk.  By 
integrating  (4.26)  twice  over  time,  the  generic  polynomial  can  be  written  as 


nk(t) 


nk{tk) 

6Atk 

+ 


+ 


(ffc+i 


t)3  + 


nk(tk+ 1) 
6  Atk 


(t  ~  tk)3 


f  nk{tk+ 1)  Atkiik(tk+ 1)\  ^ 

y  Atk  6  J  k 


( nk(tk)  _  Atkiik(tk)\  ^ 

y  Atk  6  J  fe+1 


(4.27) 


k  =  l,...,N  +  1, 


which  depends  on  the  4  unknowns:  IIk(tk),  nk(tk+i),  nk(tk),  IIk(tk+ 1). 

Notice  that  the  N  variables  qk  for  k  ^  2,  fV  +  1  are  given  via  (4.13),  while 
continuity  is  imposed  for  q2  and  qN+i  via  (4.23).  By  using  (4.14),  (4.17), 
(4.20),  the  unknowns  in  the  N  +  1  equations  in  (4.27)  reduce  to  2  (N  +  2). 
By  observing  that  the  equations  in  (4.18),  (4.21)  depend  on  q2  and  qN+i,  and 
that  qt  and  qf  are  given,  q2  and  qN+i  can  be  computed  as  a  function  of  77i(ti) 
and  TIn+i  it n+2)  ?  respectively.  Thus,  a  number  of  2(7V  +  1)  unknowns  are  left. 

By  accounting  for  (4.16),  (4.25),  and  noticing  that  in  ((4.19),  (4.22)  qt  and 
ijf  are  given,  the  unknowns  reduce  to  N. 

At  this  point,  (4.15),  (4.24)  can  be  utilized  to  write  the  system  of  N 
equations  in  N  unknowns: 


ni(t2)  =  n2(t2) 
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J7jv(ijv+i)  =  nN+i(tN+l)- 

Time-differentiation  of  (4.27)  gives  both  IIk{tk+i )  and  IIk+i{tk+i)  for  k  = 
1 , ,N,  and  thus  it  is  possible  to  write  a  system  of  linear  equations  of  the 
kind 

A[/72(t2)  nN+i(tN+i)  ]T  =  b  (4.28) 

which  presents  a  vector  b  of  known  terms  and  a  nonsingular  coefficient  matrix 
A\  the  solution  to  this  system  always  exists  and  is  unique.  It  can  be  shown 
that  the  matrix  A  has  a  tridiagonal  band  structure  of  the  type 
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which  simplifies  the  solution  to  the  system  (see  Problem  4.4).  This  matrix 
is  the  same  for  all  joints,  since  it  depends  only  on  the  time  intervals  Atk 
specified. 

An  efficient  solution  algorithm  exists  for  the  above  system  which  is  given 
by  a  forward  computation  followed  by  a  backward  computation.  From  the 
first  equation,  772(f2)  can  be  computed  as  a  function  of  Il^fts)  and  then 
substituted  in  the  second  equation,  which  then  becomes  an  equation  in  the 
unknowns  limits)  and  II This  is  carried  out  forward  by  transforming  all 
the  equations  in  equations  with  two  unknowns,  except  the  last  one  which  will 
have  77/v+i(f jv+i)  only  as  unknown.  At  this  point,  all  the  unknowns  can  be 
determined  step  by  step  through  a  backward  computation. 

The  above  sequence  of  cubic  polynomials  is  termed  spline  to  indicate 
smooth  functions  that  interpolate  a  sequence  of  given  points  ensuring  con¬ 
tinuity  of  the  function  and  its  derivatives. 

Figure  4.7  illustrates  the  time  history  of  position,  velocity  and  acceleration 
obtained  with  the  data:  <71  =  0,  <73  =  27T,  <74  =  7r/2,  qg  =  7r,  t\  =  0,  1 3  =  2, 
ti  =  3,  te  =  5,  q\  =  0,  qe  =  0.  Two  different  pairs  of  virtual  points  were 
considered  at  the  time  instants:  <2  =  0.5,  t.5  =  4.5  (solid  line  in  the  figure), 
and  t2  =  1-5,  t5  =  3.5  (dashed  line  in  the  figure),  respectively.  Notice  the 
parabolic  velocity  profile  and  the  linear  acceleration  profile.  Further,  for  the 
second  pair,  larger  values  of  acceleration  are  obtained,  since  the  relative  time 
instants  are  closer  to  those  of  the  two  intermediate  points. 

Interpolating  linear  polynomials  with  parabolic  blends 

A  simplification  in  trajectory  planning  can  be  achieved  as  follows.  Consider 
the  case  when  it  is  desired  to  interpolate  N  path  points  q±, ... ,  gjv  at  time 
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Fig.  4.7.  Time  history  of  position,  velocity  and  acceleration  with  a  timing  law  of 
cubic  splines  for  two  different  pairs  of  virtual  points 


instants  ti, . . .  ,tjv  with  linear  segments.  To  avoid  discontinuity  problems  on 
the  first  derivative  at  the  time  instants  tk,  the  function  q(t)  must  have  a 
parabolic  profile  {blend)  around  tk',  as  a  consequence,  the  entire  trajectory  is 
composed  of  a  sequence  of  linear  and  quadratic  polynomials,  which  in  turn 
implies  that  a  discontinuity  on  q{t)  is  tolerated. 

Then  let  Atk  =  tk+i  —  tk  be  the  time  distance  between  qk  and  qk+i,  and 
Atk,k+ 1  be  the  time  interval  during  which  the  trajectory  interpolating  qk  and 
qk+i  is  a  linear  function  of  time.  Also  let  qk,k+ 1  be  the  constant  velocity  and 
<jk  be  the  acceleration  in  the  parabolic  blend  whose  duration  is  At'k.  The 
resulting  trajectory  is  illustrated  in  Fig.  4.8.  The  values  of  qk,  Atk,  and  At'k 
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Fig.  4.8.  Characterization  of  a  trajectory  with  interpolating  linear  polynomials  with 
parabolic  blends 


are  assumed  to  be  given.  Velocity  and  acceleration  for  the  intermediate  points 
are  computed  as 


Qk-l,k 

qk 


qk  —  qk- 1 
Atk~  i 

qk,k+ 1  —  qk- i,fc 

M  ; 


(4.29) 

(4.30) 


these  equations  are  straightforward. 

The  first  and  last  segments  deserve  special  care.  In  fact,  if  it  is  desired  to 
maintain  the  coincidence  of  the  trajectory  with  the  first  and  last  segments, 
at  least  for  a  portion  of  time,  the  resulting  trajectory  has  a  longer  duration 
given  by  f  at  —  fi  +  {At^  +  At'N) /2,  where  go,i  =  qN,N+i  =  0  has  been  imposed 
for  computing  initial  and  final  accelerations. 

Notice  that  q(t)  reaches  none  of  the  path  points  qk  but  passes  nearby 
(Fig.  4.8).  In  this  situation,  the  path  points  are  more  appropriately  termed 
via  points',  the  larger  the  blending  acceleration,  the  closer  the  passage  to  a  via 
point. 

On  the  basis  of  the  given  qk,  Atk  and  At'k,  the  values  of  <ik-i,k  and 
are  computed  via  (4.29),  (4.30)  and  a  sequence  of  linear  polynomials  with 
parabolic  blends  is  generated.  Their  expressions  as  a  function  of  time  are  not 
derived  here  to  avoid  further  loading  of  the  analytical  presentation. 

Figure  4.9  illustrates  the  time  history  of  position,  velocity  and  acceleration 
obtained  with  the  data:  qi  =  0,  q2  =  2ir,  q$  =  7r/2,  q 4  =  n,  t\  =  0,  O  =  2, 
£3  =  3,  £4  =  5,  q-[  =  0,  <74  =  0.  Two  different  values  for  the  blend  times  have 
been  considered:  At'k  =  0.2  (solid  line  in  the  figure)  and  At'k  =  0.6  (dashed 
line  in  the  figure),  for  k  =  1, ...  ,4,  respectively.  Notice  that  in  the  first  case 
the  passage  of  q(t)  is  closer  to  the  via  points,  though  at  the  expense  of  higher 
acceleration  values. 
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Fig.  4.9.  Time  history  of  position,  velocity  and  acceleration  with  a  timing  law  of 
interpolating  linear  polynomials  with  parabolic  blends 


The  technique  presented  above  turns  out  to  be  an  application  of  the  trape¬ 
zoidal  velocity  profile  law  to  the  interpolation  problem.  If  one  gives  up  a  tra¬ 
jectory  passing  near  a  via  point  at  a  prescribed  instant  of  time,  the  use  of 
trapezoidal  velocity  profiles  allows  the  development  of  a  trajectory  planning 
algorithm  which  is  attractive  for  its  simplicity. 

In  particular,  consider  the  case  of  one  intermediate  point  only,  and  suppose 
that  trapezoidal  velocity  profiles  are  considered  as  motion  primitives  with 
the  possibility  to  specify  the  initial  and  final  point  and  the  duration  of  the 
motion  only;  it  is  assumed  that  cfo  =  q/  =  0.  If  two  segments  with  trapezoidal 
velocity  profiles  were  generated,  the  manipulator  joint  would  certainly  reach 
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the  intermediate  point,  but  it  would  be  forced  to  stop  there,  before  continuing 
the  motion  towards  the  final  point.  A  keen  alternative  is  to  start  generating 
the  second  segment  ahead  of  time  with  respect  to  the  end  of  the  first  segment, 
using  the  sum  of  velocities  (or  positions)  as  a  reference.  In  this  way,  the  joint 
is  guaranteed  to  reach  the  final  position;  crossing  of  the  intermediate  point  at 
the  specified  instant  of  time  is  not  guaranteed,  though. 

Figure  4.10  illustrates  the  time  history  of  position,  velocity  and  accelera¬ 
tion  obtained  with  the  data:  q-t  =  0,  qf  =  37r/2,  ti  =  0,  tf  =  2.  The  interme¬ 
diate  point  is  located  at  q  =  7r  with  t  =  1,  the  maximum  acceleration  values 
in  the  two  segments  are  respectively  \qc\  =  67r  and  \qc\  =  3it,  and  the  time 
anticipation  is  0.18.  As  predicted,  with  time  anticipation,  the  assigned  inter¬ 
mediate  position  becomes  a  via  point  with  the  advantage  of  an  overall  shorter 
time  duration.  Notice,  also,  that  velocity  does  not  vanish  at  the  intermediate 
point. 


4.3  Operational  Space  Trajectories 

A  joint  space  trajectory  planning  algorithm  generates  a  time  sequence  of  val¬ 
ues  for  the  joint  variables  q(t)  so  that  the  manipulator  is  taken  from  the 
initial  to  the  final  configuration,  eventually  by  moving  through  a  sequence  of 
intermediate  configurations.  The  resulting  end-effector  motion  is  not  easily 
predictable,  in  view  of  the  nonlinear  effects  introduced  by  direct  kinematics. 
Whenever  it  is  desired  that  the  end-effector  motion  follows  a  geometrically 
specified  path  in  the  operational  space,  it  is  necessary  to  plan  trajectory  exe¬ 
cution  directly  in  the  same  space.  Planning  can  be  done  either  by  interpolating 
a  sequence  of  prescribed  path  points  or  by  generating  the  analytical  motion 
primitive  and  the  relative  trajectory  in  a  punctual  way. 

In  both  cases,  the  time  sequence  of  the  values  attained  by  the  operational 
space  variables  is  utilized  in  real  time  to  obtain  the  corresponding  sequence 
of  values  of  the  joint  space  variables,  via  an  inverse  kinematics  algorithm.  In 
this  regard,  the  computational  complexity  induced  by  trajectory  generation 
in  the  operational  space  and  related  kinematic  inversion  sets  an  upper  limit 
on  the  maximum  sampling  rate  to  generate  the  above  sequences.  Since  these 
sequences  constitute  the  reference  inputs  to  the  motion  control  system,  a 
linear  microinterpolation  is  typically  carried  out.  In  this  way,  the  frequency 
at  which  reference  inputs  are  updated  is  increased  so  as  to  enhance  dynamic 
performance  of  the  system. 

Whenever  the  path  is  not  to  be  followed  exactly,  its  characterization  can 
be  performed  through  the  assignment  of  N  points  specifying  the  values  of  the 
variables  xe  chosen  to  describe  the  end-effector  pose  in  the  operational  space 
at  given  time  instants  tk,  for  k  =  1  Similar  to  what  was  presented 

in  the  above  sections,  the  trajectory  is  generated  by  determining  a  smooth 
interpolating  vector  function  between  the  various  path  points.  Such  a  function 
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Fig.  4.10.  Time  history  of  position,  velocity  and  acceleration  with  a  timing  law  of 
interpolating  linear  polynomials  with  parabolic  blends  obtained  by  anticipating  the 
generation  of  the  second  segment  of  trajectory 


can  be  computed  by  applying  to  each  component  of  xe  any  of  the  interpolation 
techniques  illustrated  in  Sect.  4.2.2  for  the  single  joint  variable. 

Therefore,  for  given  path  (or  via)  points  xe(tk),  the  corresponding  com¬ 
ponents  xei(tk),  for  i  =  1 .r  (where  r  is  the  dimension  of  the  operational 
space  of  interest)  can  be  interpolated  with  a  sequence  of  cubic  polynomials,  a 
sequence  of  linear  polynomials  with  parabolic  blends,  and  so  on. 

On  the  other  hand,  if  the  end-effector  motion  has  to  follow  a  prescribed 
trajectory  of  motion,  this  must  be  expressed  analytically.  It  is  then  necessary 
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to  refer  to  motion  primitives  defining  the  geometric  features  of  the  path  and 
time  primitives  defining  the  timing  law  on  the  path  itself. 

4.3.1  Path  Primitives 

For  the  definition  of  path  primitives  it  is  convenient  to  refer  to  the  parametric 
description  of  paths  in  space.  Then  let  p  be  a  (3  x  1)  vector  and  f(cr)  a  con¬ 
tinuous  vector  function  defined  in  the  interval  [<jj,cr/].  Consider  the  equation 

p  =  /(cr);  (4.31) 

with  reference  to  its  geometric  description,  the  sequence  of  values  of  p  with 
<t  varying  in  [<7j,<7/]  is  termed  path  in  space.  The  equation  in  (4.31)  defines 
the  parametric  representation  of  the  path  r  and  the  scalar  a  is  called  pa¬ 
rameter.  As  i t  increases,  the  point  p  moves  on  the  path  in  a  given  direction. 
This  direction  is  said  to  be  the  direction  induced  on  r  by  the  parametric 
representation  (4.31).  A  path  is  closed  when  p(<r/)  =  p(cq);  otherwise  it  is 
open. 

Let  pi  be  a  point  on  the  open  path  r  on  which  a  direction  has  been  fixed. 
The  arc  length  s  of  the  generic  point  p  is  the  length  of  the  arc  of  r  with 
extremes  p  and  p,  if  p  follows  p,,  the  opposite  of  this  length  if  p  precedes  p,. 
The  point  p4  is  said  to  be  the  origin  of  the  arc  length  (s  =  0) . 

From  the  above  presentation  it  follows  that  to  each  value  of  s  a  well- 
determined  path  point  corresponds,  and  then  the  arc  length  can  be  used  as  a 
parameter  in  a  different  parametric  representation  of  the  path  F : 

P  =  /(«);  (4.32) 

the  range  of  variation  of  the  parameter  s  will  be  the  sequence  of  arc  lengths 
associated  with  the  points  of  T. 

Consider  a  path  r  represented  by  (4.32).  Let  p  be  a  point  corresponding 
to  the  arc  length  s.  Except  for  special  cases,  p  allows  the  definition  of  three 
unit  vectors  characterizing  the  path.  The  orientation  of  such  vectors  depends 
exclusively  on  the  path  geometry,  while  their  direction  depends  also  on  the 
direction  induced  by  (4.32)  on  the  path. 

The  first  of  such  unit  vectors  is  the  tangent  unit  vector  denoted  by  t.  This 
vector  is  oriented  along  the  direction  induced  on  the  path  by  s. 

The  second  unit  vector  is  the  normal  unit  vector  denoted  by  n.  This  vector 
is  oriented  along  the  line  intersecting  p  at  a  right  angle  with  t  and  lies  in  the 
so-called  osculating  plane  O  (Fig.  4.11);  such  plane  is  the  limit  position  of  the 
plane  containing  the  unit  vector  t  and  a  point  pTf  when  p'  tends  to  p  along 
the  path.  The  direction  of  n  is  so  that  the  path  F,  in  the  neighbourhood  of  p 
with  respect  to  the  plane  containing  t  and  normal  to  n,  lies  on  the  same  side 
of  n. 

The  third  unit  vector  is  the  binormal  unit  vector  denoted  by  b.  This  vector 
is  so  that  the  frame  (£,  to,  b)  is  right-handed  (Fig.  4.11).  Notice  that  it  is  not 
always  possible  to  define  uniquely  such  a  frame. 
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Fig.  4.11.  Parametric  representation  of  a  path  in  space 


It  can  be  shown  that  the  above  three  unit  vectors  are  related  by  simple 
relations  to  the  path  representation  T  as  a  function  of  the  arc  length.  In 
particular,  it  is 


1 

d2p 

d2p 

ds2 

ds 2 

b  =  t  x  n. 


(4.33) 


Typical  path  parametric  representations  are  reported  below  which  are  useful 
for  trajectory  generation  in  the  operational  space. 


Rectilinear  path 


Consider  the  linear  segment  connecting  point  pt  to  point  p j .  The  parametric 
representation  of  this  path  is 


p(s)  =  Pi  +  iSTTj (P/  -  Pi)'  (4'34) 

Notice  that  p(0)  =  p,  and  p(\\Pf  —  pj)  =  pf.  Hence,  the  direction  induced 
on  r  by  the  parametric  representation  (4.34)  is  that  going  from  p?;  to  pj. 
Differentiating  (4.34)  with  respect  to  s  gives 


dp 

ds 


1 

\\Pf~PiW 


{: Pf-Pi ) 


(4.35) 


In  this  case  it  is  not  possible  to  define  the  frame  (t,  n ,  b)  uniquely. 


(4.36) 
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Fig.  4.12.  Parametric  representation  of  a  circle  in  space 


Circular  path 

Consider  a  circle  r  in  space.  Before  deriving  its  parametric  representation,  it 
is  necessary  to  introduce  its  significant  parameters.  Suppose  that  the  circle  is 
specified  by  assigning  (Fig.  4.12): 

•  the  unit  vector  of  the  circle  axis  r, 

•  the  position  vector  d  of  a  point  along  the  circle  axis, 

•  the  position  vector  of  a  point  on  the  circle. 

With  these  parameters,  the  position  vector  c  of  the  centre  of  the  circle  can 
be  found.  Let  S  =  pi  —  d\  for  pi  not  to  be  on  the  axis,  i.e. ,  for  the  circle  not 
to  degenerate  into  a  point,  it  must  be 

\dTr\  <  ||«||; 


in  this  case  it  is 

c  =  d+(dTr)r.  (4.37) 

It  is  now  desired  to  find  a  parametric  representation  of  the  circle  as  a  function 
of  the  arc  length.  Notice  that  this  representation  is  very  simple  for  a  suitable 
choice  of  the  reference  frame.  To  see  this,  consider  the  frame  O'—x'y'z',  where 
O'  coincides  with  the  centre  of  the  circle,  axis  x'  is  oriented  along  the  direction 
of  the  vector  pt  —  c,  axis  z'  is  oriented  along  r  and  axis  y'  is  chosen  so  as  to 
complete  a  right-handed  frame.  When  expressed  in  this  reference  frame,  the 
parametric  representation  of  the  circle  is 


p'(s) 


p  cos  (s/p) 
p  sin  (s/p)  , 

0 


(4.38) 
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where  p  =  \\p.i  —  c||  is  the  radius  of  the  circle  and  the  point  pt  has  been 
assumed  as  the  origin  of  the  arc  length.  For  a  different  reference  frame,  the 
path  representation  becomes 


p(s)  =  c  +  Rp'(s), 


(4.39) 


where  c  is  expressed  in  the  frame  O-xyz  and  R  is  the  rotation  matrix  of 
frame  O'-  x'y'z1  with  respect  to  frame  O-xyz  which,  in  view  of  (2.3),  can  be 
written  as 

R=[x'  y ’  z'J; 

x' ,  y' ,  z '  indicate  the  unit  vectors  of  the  frame  expressed  in  the  frame  O-xyz. 
Differentiating  (4.39)  with  respect  to  s  gives 


dp 

ds 


R 


—sin  (s/p) 
cos (s/p) 
0 


d2p 

ds2 


R 


-cos  (s/p)/p 
-sin  (s/p)/p 
0 


(4.40) 

(4.41) 


4.3.2  Position 


Let  xe  be  the  vector  of  operational  space  variables  expressing  the  pose  of 
the  manipulator’s  end-effector  as  in  (2.80).  Generating  a  trajectory  in  the 
operational  space  means  to  determine  a  function  xe  (t)  taking  the  end-effector 
frame  from  the  initial  to  the  final  pose  in  a  time  t /  along  a  given  path  with  a 
specific  motion  timing  law.  First,  consider  end-effector  position.  Orientation 
will  follow. 

Let  pe  =  f(s)  be  the  (3  x  1)  vector  of  the  parametric  representation  of  the 
path  r  as  a  function  of  the  arc  length  s;  the  origin  of  the  end-effector  frame 
moves  from  pt  to  p j  in  a  time  t f .  For  simplicity,  suppose  that  the  origin  of 
the  arc  length  is  at  p,  and  the  direction  induced  on  r  is  that  going  from  pt 
to  pj.  The  arc  length  then  goes  from  the  value  s  =  0  at  t  =  0  to  the  value 
s  =  Sf  (path  length)  at  t  =  tf .  The  timing  law  along  the  path  is  described  by 
the  function  s(t). 

In  order  to  find  an  analytical  expression  for  s(t),  any  of  the  above  tech¬ 
niques  for  joint  trajectory  generation  can  be  employed.  In  particular,  either  a 
cubic  polynomial  or  a  sequence  of  linear  segments  with  parabolic  blends  can 
be  chosen  for  s(t). 

It  is  worth  making  some  remarks  on  the  time  evolution  of  pe  on  T,  for  a 
given  timing  law  s(t).  The  velocity  of  point  pe  is  given  by  the  time  derivative 
of  pe 


=  st , 
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where  t  is  the  tangent  vector  to  the  path  at  point  p  in  (4.33).  Then,  s  rep¬ 
resents  the  magnitude  of  the  velocity  vector  relative  to  point  p,  taken  with 
the  positive  or  negative  sign  depending  on  the  direction  of  p  along  t.  The 
magnitude  of  p  starts  from  zero  at  t  =  0,  then  it  varies  with  a  parabolic  or 
trapezoidal  profile  as  per  either  of  the  above  choices  for  s(i),  and  finally  it 
returns  to  zero  at  t  =  tf. 

As  a  first  example,  consider  the  segment  connecting  point  pt  with  point  Pj . 
The  parametric  representation  of  this  path  is  given  by  (4.34).  Velocity  and  ac¬ 
celeration  of  pe  can  be  easily  computed  by  recalling  the  rule  of  differentiation 
of  compound  functions,  i.e., 


Pe 

Pe 


S 

\\Pf~PiW 

s 

\\pf-pi\\ 


0 Pf-Pi )  =  St 
ijPf-Pi)  =  St. 


(4.42) 

(4.43) 


As  a  further  example,  consider  a  circle  r  in  space.  From  the  parametric 
representation  derived  above,  in  view  of  (4.40),  (4.41),  velocity  and  accelera¬ 
tion  of  point  pe  on  the  circle  are 


Pe  =  R 

— ssin  (s/p) 
s  cos (s/p) 

0 

Pe  =  R 

— s2cos  (s/p)/p  —  ssin  (s/p) 
— s2sin  (s/p)/p  +  s  cos  (s/p) 

0 


(4.44) 

(4.45) 


Notice  that  the  velocity  vector  is  aligned  with  t ,  and  the  acceleration  vector 
is  given  by  two  contributions:  the  first  is  aligned  with  n  and  represents  the 
centripetal  acceleration,  while  the  second  is  aligned  with  t  and  represents  the 
tangential  acceleration. 

Finally,  consider  the  path  consisting  of  a  sequence  of  N  +  1  points, 
p0,Pi,  ■  ■  ■  ,Pi v,  connected  by  N  segments.  A  feasible  parametric  representa¬ 
tion  of  the  overall  path  is  the  following: 

N 

p.  =  +  E  n? .  (fi  -  Pj-i>’  («6> 

In  (4.46)  Sj  is  the  arc  length  associated  with  the  j-tli  segment  of  the  path, 
connecting  point  Pj-i  to  point  p^  defined  as 

{0  0  <  t  <  tj- 1 

s'(<)  I,  ,  <t<tj 

\\Pj-Pj-iW  tj<t<tf, 


(4.47) 
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where  to  =  0  and  tjsr  =  tf  are  respectively  the  initial  and  final  time  instants  of 
the  trajectory,  tj  is  the  time  instant  corresponding  to  point  Pj  and  s'(f)  can 
be  an  analytical  function  of  cubic  polynomial  type,  linear  type  with  parabolic 
blends,  and  so  forth,  which  varies  continuously  from  the  value  s'  =  0  at 
t  =  tj- 1  to  the  value  s'  =  \\pj  —  Pj-i\\  at  t  =  tj. 

The  velocity  and  acceleration  of  pe  can  be  easily  found  by  differentiat¬ 
ing  (4.46): 


N 

£ 

3  =  1 

N 

wp  P  w{pt  pi-i)  =  £s^- 
ii Pj  Pj- in  j= i 

(4.48) 

N 

N 

£ 

3= 1 

(4.49) 

where  tj  is  the  tangent  unit  vector  of  the  j-th  segment. 

Because  of  the  discontinuity  of  the  first  derivative  at  the  path  points  be¬ 
tween  two  non-aligned  segments,  the  manipulator  will  have  to  stop  and  then 
go  along  the  direction  of  the  following  segment.  Assumed  a  relaxation  of  the 
constraint  to  pass  through  the  path  points,  it  is  possible  to  avoid  a  manipu¬ 
lator  stop  by  connecting  the  segments  near  the  above  points,  which  will  then 
be  named  operational  space  via  points  so  as  to  guarantee,  at  least,  continuity 
of  the  first  derivative. 

As  already  illustrated  for  planning  of  interpolating  linear  polynomials  with 
parabolic  blends  passing  by  the  via  points  in  the  joint  space,  the  use  of  trape¬ 
zoidal  velocity  profiles  for  the  arc  lengths  allows  the  development  of  a  rather 
simple  planning  algorithm 

In  detail,  it  will  be  sufficient  to  properly  anticipate  the  generation  of  the 
single  segments,  before  the  preceding  segment  has  been  completed.  This  leads 
to  modifying  (4.47)  as  follows: 

!0  0  <  t  <  tj-!  -  Atj 

s'j{t  +  Atj)  tj- 1  -  Atj  <t  <tj  -  Atj  (4.50) 

II  Pj  -  Pj-  ill  tj  ~  Atj  <t<tf-  AtN, 

where  Atj  is  the  time  advance  at  which  the  j-th  segment  is  generated,  which 
can  be  recursively  evaluated  as 

Atj  =  Atj—!  T  tttj , 

with  j  =  1 , ...  ,7V  e  Ato  =  0.  Notice  that  this  time  advance  is  given  by  the 
sum  of  two  contributions:  the  former,  Atj-!,  accounts  for  the  sum  of  the  time 
advances  at  which  the  preceding  segments  have  been  generated,  while  the 
latter,  5tj ,  is  the  time  advance  at  which  the  generation  of  the  current  segment 
starts. 
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4.3.3  Orientation 

Consider  now  end-effector  orientation.  Typically,  this  is  specified  in  terms  of 
the  rotation  matrix  of  the  (time-varying)  end-effector  frame  with  respect  to 
the  base  frame.  As  is  well  known,  the  three  columns  of  the  rotation  matrix 
represent  the  three  unit  vectors  of  the  end-effector  frame  with  respect  to  the 
base  frame.  To  generate  a  trajectory,  however,  a  linear  interpolation  on  the 
unit  vectors  ne,  se,  ae  describing  the  initial  and  final  orientation  does  not 
guarantee  orthonormality  of  the  above  vectors  at  each  instant  of  time. 


Euler  angles 

In  view  of  the  above  difficulty,  for  trajectory  generation  purposes,  orientation 
is  often  described  in  terms  of  the  Euler  angles  triplet  cf)e  =  (ip,  i9,  ip)  for  which 
a  timing  law  can  be  specified.  Usually,  (pe  moves  along  the  segment  connecting 
its  initial  value  (p>i  to  its  final  value  (pf.  Also  in  this  case,  it  is  convenient  to 
choose  a  cubic  polynomial  or  a  linear  segment  with  parabolic  blends  timing 
law.  In  this  way,  in  fact,  the  angular  velocity  u)e  of  the  time-varying  frame, 
which  is  related  to  cpe  by  the  linear  relationship  (3.64),  will  have  continuous 
magnitude. 

Therefore,  for  given  (p)i  and  <pf  and  timing  law,  the  position,  velocity  and 
acceleration  profiles  are 

4=PF«w',“'w  (4-51) 

where  the  timing  law  for  s(t)  has  to  be  specified.  The  three  unit  vectors  of  the 
end-effector  frame  can  be  computed  —  with  reference  to  Euler  angles  ZYZ 
—  as  in  (2.18),  the  end-effector  frame  angular  velocity  as  in  (3.64),  and  the 
angular  acceleration  by  differentiating  (3.64)  itself. 


Angle  and  axis 

An  alternative  way  to  generate  a  trajectory  for  orientation  of  clearer  inter¬ 
pretation  in  the  Cartesian  space  can  be  derived  by  resorting  to  the  the  angle 
and  axis  description  presented  in  Sect.  2.5.  Given  two  coordinate  frames  in 
the  Cartesian  space  with  the  same  origin  and  different  orientation,  it  is  always 
possible  to  determine  a  unit  vector  so  that  the  second  frame  can  be  obtained 
from  the  first  frame  by  a  rotation  of  a  proper  angle  about  the  axis  of  such 
unit  vector. 
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Let  Rj  and  Rf  denote  respectively  the  rotation  matrices  of  the  initial 
frame  Oi-XiUiZi  and  the  final  frame  Of-XfijfZf ,  both  with  respect  to  the 
base  frame.  The  rotation  matrix  between  the  two  frames  can  be  computed  by 
recalling  that  Rf  =  RiR) ;  the  expression  in  (2.5)  leads  to 


R'f  =  RjRf  = 


rn 

r  12 

r\3 

r2 1 

r22 

r23 

r3  i 

r32 

T33 

If  the  matrix  Rl(t)  is  defined  to  describe  the  transition  from  R,  to  Rf,  it 
must  be  R‘( 0)  =  I  and  R'  (tf)  =  Rlf.  Hence,  the  matrix  R'f  can  be  expressed 
as  the  rotation  matrix  about  a  fixed  axis  in  space;  the  unit  vector  rl  of  the 
axis  and  the  angle  of  rotation  fif  can  be  computed  by  using  (2.27): 


q  _i  ( ru  +  r22  +  r33  -  1\ 

i?/  =  coy  ^ - - - j 

r3 2  -  r23 
rn  -  r3i 
r2i  -  r12 


1 

2  sin  f 


(4.52) 

(4.53) 


for  sin  $ f  ^  0. 

The  matrix  Rl(t)  can  be  interpreted  as  a  matrix  Rl  ($(f) ,  rl)  and  computed 
via  (2.25);  it  is  then  sufficient  to  assign  a  timing  law  to  $,  of  the  type  of  those 
presented  for  the  single  joint  with  t?(0)  =  0  and  $(</)  =  $/,  and  compute  the 
components  of  rl  from  (4.52).  Since  rl  is  constant,  the  resulting  velocity  and 
acceleration  are  respectively 


wi  =  tiri  (4.54) 

(4.55) 


Finally,  in  order  to  characterize  the  end-effector  orientation  trajectory  with 
respect  to  the  base  frame,  the  following  transformations  are  needed: 

Re(t)  =  RiR\t) 
ue(t)  =  RiU)l{t) 
ue{t)  =  Rid>l(t). 


Once  a  path  and  a  trajectory  have  been  specified  in  the  operational  space 
in  terms  of  pe{t)  and  </>e(f)  or  Re(t),  inverse  kinematics  techniques  can  be 
used  to  find  the  corresponding  trajectories  in  the  joint  space  q(t). 
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Trajectory  planning  for  robot  manipulators  has  been  addressed  since  the  first 
works  in  the  field  of  robotics  [178].  The  formulation  of  the  interpolation  prob¬ 
lem  of  the  path  points  by  means  of  different  classes  of  functions  has  been 
suggested  in  [26]. 


Problems  189 


The  generation  of  motion  trajectories  through  sequences  of  points  in  the 
joint  space  using  splines  is  due  to  [131].  Alternative  formulations  for  this 
problem  are  found  in  [56].  For  a  complete  treatment  of  splines,  including 
geometric  properties  and  computational  aspects,  see  [54].  In  [155]  a  survey 
on  the  functions  employed  for  trajectory  planning  of  a  single  motion  axis 
is  given,  which  accounts  for  performance  indices  and  effects  of  unmodelled 
flexible  dynamics. 

Cartesian  space  trajectory  planning  and  the  associated  motion  control 
problem  have  been  originally  treated  in  [179].  The  systematic  management 
of  the  motion  by  the  via  points  using  interpolating  linear  polynomials  with 
parabolic  blends  has  been  proposed  in  [229].  A  detailed  presentation  of  the 
general  aspects  of  the  geometric  primitives  that  can  be  utilized  in  robotics  to 
define  Cartesian  space  paths  can  be  found  in  the  computer  graphics  text  [73]. 


Problems 

4.1.  Compute  the  joint  trajectory  from  q( 0)  =  1  to  q( 2)  =  4  with  null  initial 
and  final  velocities  and  accelerations. 

4.2.  Compute  the  timing  law  q(t)  for  a  joint  trajectory  with  velocity  profile 
of  the  type  q(t )  =  k(  1  —  cos  (at))  from  q( 0)  =  0  to  q( 2)  =  3. 

4.3.  Given  the  values  for  the  joint  variable:  q( 0)  =  0,  q( 2)  =  2,  and  q( 4)  =  3, 
compute  the  two  fifth-order  interpolating  polynomials  with  continuous  veloc¬ 
ities  and  accelerations. 

4.4.  Show  that  the  matrix  A  in  (4.28)  has  a  tridiagonal  band  structure. 

4.5.  Given  the  values  for  the  joint  variable:  q( 0)  =  0,  q( 2)  =  2,  and  q( 4)  =  3, 
compute  the  cubic  interpolating  spline  with  null  initial  and  final  velocities  and 
accelerations. 

4.6.  Given  the  values  for  the  joint  variable:  q( 0)  =  0,  q( 2)  =  2,  and  q( 4)  =  3, 
find  the  interpolating  polynomial  with  linear  segments  and  parabolic  blends 
with  null  initial  and  final  velocities. 

4.7.  Find  the  timing  law  p(t)  for  a  Cartesian  space  rectilinear  path  with  trape¬ 
zoidal  velocity  profile  from  p( 0)  =  [0  0.5  0]T  to  p( 2)  =  [1  —0.5  0]T. 

4.8.  Find  the  timing  law  p(t)  for  a  Cartesian  space  circular  path  with  trape¬ 
zoidal  velocity  profile  from  p(0)  =  [0  0.5  1]T  to  p( 2)  =  [0  —0.5  1]T; 
the  circle  is  located  in  the  plane  x  =  0  with  centre  at  c  =  [0  0  1]T  and 
radius  p  =  0.5,  and  is  executed  clockwise  for  an  observer  aligned  with  x. 


5 


Actuators  and  Sensors 


In  this  chapter,  two  basic  robot  components  are  treated:  actuators  and  sen¬ 
sors.  In  the  first  part,  the  features  of  an  actuating  system  are  presented  in 
terms  of  the  power  supply,  power  amplifier,  servomotor  and  transmission.  In 
view  of  their  control  versatility,  two  types  of  servomotors  are  used,  namely, 
electric  servomotors  for  actuating  the  joints  of  small  and  medium  size  ma¬ 
nipulators,  and  hydraulic  servomotors  for  actuating  the  joints  of  large  size 
manipulators.  The  models  describing  the  input/output  relationship  for  such 
servomotors  are  derived,  together  with  the  control  schemes  of  the  drives.  The 
electric  servomotors  are  also  employed  to  actuate  the  wheels  of  the  mobile 
robots,  which  will  be  dealt  with  in  Chap.  11.  Successively,  proprioceptive  sen¬ 
sors  are  presented  which  allow  measurement  of  the  quantities  characterizing 
the  internal  state  of  the  manipulator,  namely,  encoders  and  resolvers  for  joint 
position  measurement,  tachometers  for  joint  velocity  measurement;  further, 
exteroceptive  sensors  are  presented  including  force  sensors  for  end-effector 
force  measurement,  distance  sensors  for  detection  of  objects  in  the  workspace, 
and  vision  sensors  for  the  measurement  of  the  characteristic  parameters  of 
such  objects,  whenever  the  manipulator  interacts  with  the  environment. 


5.1  Joint  Actuating  System 

The  motion  imposed  to  a  manipulator’s  joint  is  realized  by  an  actuating  system 
which  in  general  consists  of: 

•  a  power  supply, 

•  a  power  amplifier, 

•  a  servomotor, 

•  a  transmission. 

The  connection  between  the  various  components  is  illustrated  in  Fig.  5.1 
where  the  exchanged  powers  are  shown.  To  this  end,  recall  that  power  can 
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Fig.  5.1.  Components  of  a  joint  actuating  system 


always  be  expressed  as  the  product  of  a  flow  and  a  force  quantity,  whose 
physical  context  allows  the  specification  of  the  nature  of  the  power  (mechan¬ 
ical,  electric,  hydraulic,  or  pneumatic). 

In  terms  of  a  global  input/output  relationship,  Pc  denotes  the  (usually 
electric)  power  associated  with  the  control  law  signal,  whereas  Pu  represents 
the  mechanical  power  required  to  the  joint  to  actuate  the  motion.  The  in¬ 
termediate  connections  characterize  the  supply  power  Pa  of  the  motor  (of 
electric,  hydraulic,  or  pneumatic  type),  the  power  provided  by  the  primary 
source  Pp  of  the  same  physical  nature  as  that  of  Pa,  and  the  mechanical  power 
Pm  developed  by  the  motor.  Moreover,  Pda,  Pds  and  Pdt  denote  the  powers 
lost  for  dissipation  in  the  conversions  performed  respectively  by  the  amplifier, 
motor  and  transmission. 

To  choose  the  components  of  an  actuating  system,  it  is  worth  starting 
from  the  requirements  imposed  on  the  mechanical  power  Pu  by  the  force  and 
velocity  that  describe  the  joint  motion. 

5.1.1  Transmissions 

The  execution  of  joint  motions  of  a  manipulator  demands  low  speeds  with 
high  torques.  In  general,  such  requirements  do  not  allow  an  effective  use  of  the 
mechanical  features  of  servomotors,  which  typically  provide  high  speeds  with 
low  torques  in  optimal  operating  conditions.  It  is  then  necessary  to  interpose 
a  transmission  {gear)  to  optimize  the  transfer  of  mechanical  power  from  the 
motor  {Pm)  to  the  joint  (Pu).  During  this  transfer,  the  power  Pdt  is  dissipated 
as  a  result  of  friction. 

The  choice  of  the  transmission  depends  on  the  power  requirements,  the 
kind  of  desired  motion,  and  the  allocation  of  the  motor  with  respect  to  the 
joint.  In  fact,  the  transmission  allows  the  outputs  of  the  motor  to  be  trans¬ 
formed  both  quantitatively  (velocity  and  torque)  and  qualitatively  (a  rota¬ 
tional  motion  about  the  motor  axis  into  a  translational  motion  of  the  joint). 
Also,  it  allows  the  static  and  dynamic  performance  of  a  manipulator  to  be  op¬ 
timized,  by  reducing  the  effective  loads  when  the  motor  is  located  upstream 
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of  the  joint;  for  instance,  if  some  motors  are  mounted  to  the  base  of  the  robot, 
the  total  weight  of  the  manipulator  is  decreased  and  the  power-to-weight  ratio 
is  increased. 

The  following  transmissions  are  typically  used  for  industrial  robots: 

•  Spur  gears  that  modify  the  characteristics  of  the  rotational  motion  of  the 
motor  by  changing  the  axis  of  rotation  and/or  by  translating  the  appli¬ 
cation  point;  spur  gears  are  usually  constructed  with  wide  cross-section 
teeth  and  squat  shafts. 

•  Lead  screws  that  convert  rotational  motion  of  the  motor  into  translational 
motion,  as  needed  for  actuation  of  prismatic  joints;  in  order  to  reduce  fric¬ 
tion,  ball  screws  are  usually  employed  that  are  preloaded  so  as  to  increase 
stiffness  and  decrease  backlash. 

•  Timing  belts  and  chains  which  are  equivalent  from  a  kinematic  viewpoint 
and  are  employed  to  locate  the  motor  remotely  from  the  axis  of  the  ac¬ 
tuated  joint.  The  stress  on  timing  belts  may  cause  strain,  and  then  these 
are  used  in  applications  requiring  high  speeds  and  low  forces.  On  the  other 
hand,  chains  are  used  in  applications  requiring  low  speeds,  since  their  large 
mass  may  induce  vibration  at  high  speeds. 

On  the  assumption  of  rigid  transmissions  with  no  backlash,  the  relation¬ 
ship  between  input  forces  (velocities)  and  output  forces  (velocities)  is  purely 
proportional. 

The  mechanical  features  of  the  motor  used  for  an  actuating  system  may 
sometimes  allow  a  direct  connection  of  the  motor  to  the  joint  without  the  use 
of  any  transmission  element  ( direct  drive).  The  drawbacks  due  to  transmis¬ 
sion  elasticity  and  backlash  are  thus  eliminated,  although  more  sophisticated 
control  algorithms  are  required,  since  the  absence  of  reduction  gears  does  not 
allow  the  nonlinear  coupling  terms  in  the  dynamic  model  to  be  neglected. 
The  use  of  direct-drive  actuating  systems  is  not  yet  popular  for  industrial 
manipulators,  in  view  of  the  cost  and  size  of  the  motors  as  well  as  of  control 
complexity. 

5.1.2  Servomotors 

Actuation  of  joint  motions  is  entrusted  to  motors  which  allow  the  realization 
of  a  desired  motion  for  the  mechanical  system.  Concerning  the  kind  of  input 
power  Pa,  motors  can  be  classified  into  three  groups: 

•  Pneumatic  motors  which  utilize  the  pneumatic  energy  provided  by  a  com¬ 
pressor  and  transform  it  into  mechanical  energy  by  means  of  pistons  or 
turbines. 

•  Hydraulic  motors  which  transform  the  hydraulic  energy  stored  in  a  reser¬ 
voir  into  mechanical  energy  by  means  of  suitable  pumps. 

•  Electric  motors  whose  primary  supply  is  the  electric  energy  available  from 
the  electric  distribution  system. 
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A  portion  of  the  input  power  Pa  is  converted  to  output  as  mechanical 
power  Pm,  and  the  rest  (Pds)  is  dissipated  because  of  mechanical,  electric, 
hydraulic,  or  pneumatic  loss. 

The  motors  employed  in  robotics  are  the  evolution  of  the  motors  em¬ 
ployed  in  industrial  automation  having  powers  ranging  from  about  10  W  to 
about  10  kW.  For  the  typical  performance  required,  such  motors  should  have 
the  following  requirements  with  respect  to  those  employed  in  conventional 
applications: 

•  low  inertia  and  high  power-to-weight  ratio, 

•  possibility  of  overload  and  delivery  of  impulse  torques, 

•  capability  to  develop  high  accelerations, 

•  wide  velocity  range  (from  1  to  1000  revolutes/min), 

•  high  positioning  accuracy  (at  least  1/1000  of  a  circle), 

•  low  torque  ripple  so  as  to  guarantee  continuous  rotation  even  at  low  speed. 

These  requirements  are  enhanced  by  the  good  trajectory  tracking  and 
positioning  accuracy  demanded  for  an  actuating  system  for  robots,  and  thus 
the  motor  must  play  the  role  of  a  servomotor.  In  this  respect,  pneumatic 
motors  are  difficult  to  control  accurately,  in  view  of  the  unavoidable  fluid 
compressibility  errors.  Therefore,  they  are  not  widely  employed,  if  not  for 
the  actuation  of  the  typical  opening  and  closing  motions  of  the  jaws  in  a 
gripper  tool,  then  for  the  actuation  of  simple  arms  used  in  applications  where 
continuous  motion  control  is  not  of  concern. 

The  most  employed  motors  in  robotics  applications  are  electric  servomo¬ 
tors.  Among  them,  the  most  popular  are  permanent-magnet  direct-current 
(DC)  servomotors  and  brushless  DC  servomotors,  in  view  of  their  good  con¬ 
trol  flexibility. 

The  permanent-magnet.  DC  servomotor  consists  of: 

•  A  stator  coil  that  generates  magnetic  flux;  this  generator  is  always  a  per¬ 
manent  magnet  made  by  ferromagnetic  ceramics  or  rare  earths  (high  fields 
in  contained  space). 

•  An  armature  that  includes  the  current-carrying  winding  that  surrounds  a 
rotary  ferromagnetic  core  (rotor). 

•  A  commutator  that  provides  an  electric  connection  by  means  of  brushes 
between  the  rotating  armature  winding  and  the  external  feed  winding, 
according  to  a  commutation  logic  determined  by  the  rotor  motion. 

The  brushless  DC  servomotor  consists  of: 

•  A  rotating  coil  (rotor)  that  generates  magnetic  flux;  this  generator  is  a 
permanent  magnet  made  by  ferromagnetic  ceramics  or  rare  earths. 

•  A  stationary  armature  (stator)  made  by  a  polyphase  winding. 

•  A  static  commutator  that,  on  the  basis  of  the  signals  provided  by  a  posi¬ 
tion  sensor  located  on  the  motor  shaft,  generates  the  feed  sequence  of  the 
armature  winding  phases  as  a  function  of  the  rotor  motion. 


5.1  Joint  Actuating  System  195 


With  reference  to  the  above  details  of  constructions,  a  comparison  be¬ 
tween  the  operating  principle  of  a  permanent-magnet  DC  and  a  brushless  DC 
servomotor  leads  to  the  following  considerations. 

In  the  brushless  DC  motor,  by  means  of  the  rotor  position  sensor,  the 
winding  orthogonal  to  the  magnetic  field  of  the  coil  is  found;  then,  feeding  the 
winding  makes  the  rotor  rotate.  As  a  consequence  of  rotation,  the  electronic 
control  module  commutes  the  feeding  on  the  winding  of  the  various  phases  in 
such  a  way  that  the  resulting  field  at  the  armature  is  always  kept  orthogonal  to 
that  of  the  coil.  As  regards  electromagnetic  interaction,  such  a  motor  operates 
in  a  way  similar  to  that  of  a  permanent-magnet  DC  motor  where  the  brushes 
are  at  an  angle  of  7r/2  with  respect  to  the  direction  of  the  excitation  flux.  In 
fact,  feeding  the  armature  coil  makes  the  rotor  rotate,  and  commutation  of 
brushes  from  one  plate  of  the  commutator  to  the  other  allows  the  rotor  to  be 
maintained  in  rotation.  The  role  played  by  the  brushes  and  commutator  in 
a  permanent-magnet  DC  motor  is  analogous  to  that  played  by  the  position 
sensor  and  electronic  control  module  in  a  brushless  DC  motor. 

The  main  reason  for  using  a  brushless  DC  motor  is  to  eliminate  the  prob¬ 
lems  due  to  mechanical  commutation  of  the  brushes  in  a  permanent-magnet 
DC  motor.  In  fact,  the  presence  of  the  commutator  limits  the  performance 
of  a  permanent-magnet  DC  motor,  since  this  provokes  electric  loss  due  to 
voltage  drops  at  the  contact  between  the  brushes  and  plates,  and  mechani¬ 
cal  loss  due  to  friction  and  arcing  during  commutation  from  one  plate  to  the 
next  one  caused  by  the  inductance  of  the  winding.  The  elimination  of  the 
causes  provoking  such  inconveniences,  i.e. ,  the  brushes  and  plates,  allows  an 
improvement  of  motor  performance  in  terms  of  higher  speeds  and  less  material 
wear. 

The  inversion  between  the  functions  of  stator  and  rotor  leads  to  further 
advantages.  The  presence  of  a  winding  on  the  stator  instead  of  the  rotor  fa¬ 
cilitates  heat  disposal.  The  absence  of  a  rotor  winding,  together  with  the  pos¬ 
sibility  of  using  rare-earth  permanent  magnets,  allows  construction  of  more 
compact  rotors  which  are,  in  turn,  characterized  by  a  low  moment  of  iner¬ 
tia.  Therefore,  the  size  of  a  brushless  DC  motor  is  smaller  than  that  of  a 
permanent-magnet  DC  motor  of  the  same  power;  an  improvement  of  dynamic 
performance  can  also  be  obtained  by  using  a  brushless  DC  motor.  For  the 
choice  of  the  most  suitable  servomotor  for  a  specific  application,  the  cost 
factor  plays  a  relevant  role. 

Not  uncommon  are  also  stepper  motors.  These  actuators  are  controlled 
by  suitable  excitation  sequences  and  their  operating  principle  does  not  re¬ 
quire  measurement  of  motor  shaft  angular  position.  The  dynamic  behaviour 
of  stepper  motors  is  greatly  influenced  by  payload,  though.  Also,  they  induce 
vibration  of  the  mechanical  structure  of  the  manipulator.  Such  inconveniences 
confine  the  use  of  stepper  motors  to  the  field  of  micromanipulators,  for  which 
low-cost  implementation  prevails  over  the  need  for  high  dynamic  performance. 

A  certain  number  of  applications  features  the  employment  of  hydraulic 
servomotors,  which  are  based  on  the  simple  operating  principle  of  volume 
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variation  under  the  action  of  compressed  fluid.  From  a  construction  viewpoint, 
they  are  characterized  by  one  or  more  chambers  made  by  pistons  (cylinders 
reciprocating  in  tubular  housings).  Linear  servomotors  have  a  limited  range 
and  are  constituted  by  a  single  piston.  Rotary  servomotors  have  unlimited 
range  and  are  constituted  by  several  pistons  (usually  an  odd  number)  with  an 
axial  or  radial  disposition  with  respect  to  the  motor  axis  of  rotation.  These 
servomotors  offer  a  static  and  dynamic  performance  comparable  with  that 
offered  by  electric  servomotors. 

The  differences  between  electric  and  hydraulic  servomotors  can  be  funda¬ 
mentally  observed  from  a  plant  viewpoint.  In  this  respect,  electric  servomotors 
present  the  following  advantages: 

•  widespread  availability  of  power  supply, 

•  low  cost  and  wide  range  of  products, 

•  high  power  conversion  efficiency, 

•  easy  maintenance, 

•  no  pollution  of  working  environment. 

Instead,  they  present  the  following  limitations: 

•  burnout  problems  at  static  situations  caused  by  the  effect  of  gravity  on 
the  manipulator;  emergency  brakes  are  then  required, 

•  need  for  special  protection  when  operating  in  flammable  environments. 

Hydraulic  servomotors  present  the  following  drawbacks: 

•  need  for  a  hydraulic  power  station, 

•  high  cost,  narrow  range  of  products,  and  difficulty  of  miniaturization, 

•  low  power  conversion  efficiency, 

•  need  for  operational  maintenance, 

•  pollution  of  working  environment  due  to  oil  leakage. 

In  their  favour  it  is  worth  pointing  out  that  they: 

•  do  not  suffer  from  burnout  in  static  situations, 

•  are  self-lubricated  and  the  circulating  fluid  facilitates  heat  disposal, 

•  are  inherently  safe  in  harmful  environments, 

•  have  excellent  power-to-weight  ratios. 

From  an  operational  viewpoint,  it  can  be  observed  that: 

•  Both  types  of  servomotors  have  a  good  dynamic  behaviour,  although  the 
electric  servomotor  has  greater  control  flexibility.  The  dynamic  behaviour 
of  a  hydraulic  servomotor  depends  on  the  temperature  of  the  compressed 
fluid. 

•  The  electric  servomotor  is  typically  characterized  by  high  speeds  and  low 
torques,  and  as  such  it  requires  the  use  of  gear  transmissions  (causing 
elasticity  and  backlash).  On  the  other  hand,  the  hydraulic  servomotor  is 
capable  of  generating  high  torques  at  low  speeds. 
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In  view  of  the  above  remarks,  hydraulic  servomotors  are  specifically  em¬ 
ployed  for  manipulators  that  have  to  carry  heavy  payloads;  in  this  case,  not 
only  is  the  hydraulic  servomotor  the  most  suitable  actuator,  but  also  the  cost 
of  the  plant  accounts  for  a  reduced  percentage  on  the  total  cost  of  the  manip¬ 
ulation  system. 

5.1.3  Power  Amplifiers 

The  power  amplifier  has  the  task  of  modulating,  under  the  action  of  a  control 
signal,  the  power  flow  which  is  provided  by  the  primary  supply  and  has  to  be 
delivered  to  the  actuators  for  the  execution  of  the  desired  motion.  In  other 
words,  the  amplifier  takes  a  fraction  of  the  power  available  at  the  source  which 
is  proportional  to  the  control  signal;  then  it  transmits  this  power  to  the  motor 
in  terms  of  suitable  force  and  flow  quantities. 

The  inputs  to  the  amplifier  are  the  power  taken  from  the  primary  source 
Pp  and  the  power  associated  with  the  control  signal  Pc.  The  total  power  is 
partly  delivered  to  the  actuator  (Pa)  and  partly  lost  in  dissipation  (Pda)- 

Given  the  typical  use  of  electric  and  hydraulic  servomotors,  the  operational 
principles  of  the  respective  amplifiers  are  discussed. 

To  control  an  electric  servomotor ,  it  is  necessary  to  provide  it  with  a 
voltage  or  current  of  suitable  form  depending  on  the  kind  of  servomotor  em¬ 
ployed.  Voltage  (or  current)  is  direct  for  permanent-magnet  DC  servomotors, 
while  it  is  alternating  for  brushless  DC  servomotors.  The  value  of  voltage  for 
permanent-magnet  DC  servomotors  or  the  values  of  voltage  and  frequency  for 
brushless  DC  servomotors  are  determined  by  the  control  signal  of  the  ampli¬ 
fier,  so  as  to  make  the  motor  execute  the  desired  motion. 

For  the  power  ranges  typically  required  by  joint  motions  (of  the  order 
of  a  few  kilowatts),  transistor  amplifiers  are  employed  which  are  suitably 
switched  by  using  pulse-width  modulation  (PWM)  techniques.  They  allow  the 
achievement  of  a  power  conversion  efficiency  Pa/(Pp  +  Pc)  greater  than  0.9 
and  a  power  gain  Paj Pc  of  the  order  of  106.  The  amplifiers  employed  to  con¬ 
trol  permanent-magnet  DC  servomotors  are  DC-to-DC  converters  (choppers), 
whereas  those  employed  to  control  brushless  DC  servomotors  are  DC-to-AC 
converters  (inverters). 

Control  of  a  hydraulic  servomotor  is  performed  by  varying  the  flow  rate  of 
the  compressed  fluid  delivered  to  the  motor.  The  task  of  modulating  the  flow 
rate  is  typically  entrusted  to  an  interface  (electro- hydraulic  servovalve).  This 
allows  a  relationship  to  be  established  between  the  electric  control  signal  and 
the  position  of  a  distributor  which  is  able  to  vary  the  flow  rate  of  the  fluid 
transferred  from  the  primary  source  to  the  motor.  The  electric  control  signal 
is  usually  current-amplified  and  feeds  a  solenoid  which  moves  (directly  or  in¬ 
directly)  the  distributor,  whose  position  is  measured  by  a  suitable  transducer. 
In  this  way,  a  position  servo  on  the  valve  stem  is  obtained  which  reduces 
occurrence  of  any  stability  problem  that  may  arise  on  motor  control.  The 
magnitude  of  the  control  signal  determines  the  flow  rate  of  the  compressed 
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fluid  through  the  distributor,  according  to  a  characteristic  which  is  possibly 
made  linear  by  means  of  a  keen  mechanical  design. 

5.1.4  Power  Supply 

The  task  of  the  power  supply  is  to  supply  the  primary  power  to  the  amplifier 
which  is  needed  for  operation  of  the  actuating  system. 

In  the  case  of  electric  servomotors,  the  power  supply  consists  of  a  trans¬ 
former  and  a  typically  uncontrolled  bridge  rectifier.  These  allow  the  alter¬ 
nating  voltage  available  from  the  distribution  to  be  converted  into  a  direct 
voltage  of  suitable  magnitude  which  is  required  to  feed  the  power  amplifier. 

In  the  case  of  hydraulic  servomotors,  the  power  supply  is  obviously  more 
complex.  In  fact,  a  gear  or  piston  pump  is  employed  to  compress  the  fluid 
which  is  driven  by  a  primary  motor  operating  at  constant  speed,  typically  a 
three-phase  nonsynchronous  motor.  To  reduce  the  unavoidable  pressure  oscil¬ 
lations  provoked  by  a  flow  rate  demand  depending  on  operational  conditions 
of  the  motor,  a  reservoir  is  interfaced  to  store  hydraulic  energy.  Such  a  reser¬ 
voir,  in  turn,  plays  the  same  role  as  the  filter  capacitor  used  at  the  output  of  a 
bridge  rectifier.  The  hydraulic  power  station  is  completed  by  the  use  of  various 
components  (filters,  pressure  valves,  and  check  valves)  that  ensure  proper  op¬ 
eration  of  the  system.  Finally,  it  can  be  inferred  how  the  presence  of  complex 
hydraulic  circuits  operating  at  high  pressures  (of  the  order  of  100  atm)  causes 
an  appreciable  pollution  of  the  working  environment. 


5.2  Drives 

This  section  presents  the  operation  of  the  electric  drives  and  the  hydraulic 
drives  for  the  actuation  of  a  manipulator’s  joints.  Starting  from  the  math¬ 
ematical  models  describing  the  dynamic  behaviour,  the  block  schemes  are 
derived  which  allow  an  emphasis  on  the  control  features  and  the  effects  of  the 
use  of  a  mechanical  transmission. 

5.2.1  Electric  Drives 

From  a  modelling  viewpoint,  a  permanent-magnet  DC  motor  and  a  brushless 
DC  motor  provided  with  the  commutation  module  and  position  sensor  can  be 
described  by  the  same  differential  equations.  In  the  domain  of  the  complex 
variable  s,  the  electric  balance  of  the  armature  is  described  by  the  equations 

Va  =  (. Ra  +  sLa)Ia  +  Vg  (5.1) 

Vg  =  kvfim  (5.2) 

where  Va  and  Ia  respectively  denote  armature  voltage  and  current,  Ra  and 
La  are  respectively  the  armature  resistance  and  inductance,  and  Vg  denotes 
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Fig.  5.2.  Block  scheme  of  an  electric  drive 


the  back  electromotive  force  which  is  proportional  to  the  angular  velocity  f2m 
through  the  voltage  constant  kv  that  depends  on  the  construction  details  of 
the  motor  as  well  as  on  the  magnetic  flux  of  the  coil. 

The  mechanical  balance  is  described  by  the  equations 

Cm  =  ( slm  +  Fm)f2m  +  Ci  (5.3) 

Cm  =  ktIa  (5.4) 


where  Cm  and  Ci  respectively  denote  the  driving  torque  and  load  reaction 
torque,  Im  and  Fm  are  respectively  the  moment  of  inertia  and  viscous  friction 
coefficient  at  the  motor  shaft,  and  the  torque  constant  kt  is  numerically  equal 
to  kv  in  the  SI  unit  system  for  a  compensated  motor. 

Concerning  the  power  amplifier,  the  input/output  relationship  between 
the  control  voltage  Vc  and  the  armature  voltage  Va  is  given  by  the  transfer 
function 


Va  =  Gv 
Vc  1  +  sTv 


(5.5) 


where  Gv  denotes  the  voltage  gain  and  Tv  is  a  time  constant  that  can  be 
neglected  with  respect  to  the  other  time  constants  of  the  system.  In  fact,  by 
using  a  modulation  frequency  in  the  range  of  10  to  100  kHz,  the  time  constant 
of  the  amplifier  is  in  the  range  of  10-5  to  10-4)  s. 

The  block  scheme  of  the  servomotor  with  power  amplifier  ( electric  drive ) 
is  illustrated  in  Fig.  5.2.  In  such  a  scheme,  besides  the  blocks  corresponding  to 
the  above  relations,  there  is  an  armature  current  feedback  loop  where  current 
is  thought  of  as  measured  by  a  transducer  ki  between  the  power  amplifier  and 
the  armature  winding  of  the  motor.  Further,  the  scheme  features  a  current 
regulator  Cj(s)  as  well  as  an  element  with  a  nonlinear  saturation  characteris¬ 
tic.  The  aim  of  such  feedback  is  twofold.  On  one  hand,  the  voltage  V'.  plays 
the  role  of  a  current  reference  and  thus,  by  means  of  a  suitable  choice  of  the 
regulator  C;(s),  the  lag  between  the  current  Ia  and  the  voltage  V'c  can  be 
reduced  with  respect  to  the  lag  between  Ia  and  Vc.  On  the  other  hand,  the 
introduction  of  a  saturation  nonlinearity  allows  the  limitation  of  the  magni- 
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Fig.  5.3.  Block  scheme  of  an  electric  drive  as  a  velocity-controlled  generator 


tude  of  V'c,  and  then  it  works  like  a  current  limit  which  ensures  protection  of 
the  power  amplifier  whenever  abnormal  operating  conditions  occur. 

The  choice  of  the  regulator  Ci(s )  of  the  current  loop  allows  a  velocity- 
controlled  or  torque-controlled  behaviour  to  be  obtained  from  the  electric 
drive,  depending  on  the  values  attained  by  the  loop  gain.  In  fact,  in  the  case 
of  ki  =  0,  recalling  that  the  mechanical  viscous  friction  coefficient  is  negligible 
with  respect  to  the  electrical  friction  coefficient 

Fm  «  (5.6) 

assuming  a  unit  gain  constant  for  C)  (s) 1  and  Ci  =  0  yields 

^  GV  /  /r  7\ 

Um  ~  ~j—vc  (5.7) 

and  thus  the  drive  system  behaves  like  a  velocity- controlled  generator. 

Instead,  when  ki  yf  0,  choosing  a  large  loop  gain  for  the  current  loop 
( Kki  Ra)  leads  at  steady  state  to 


c 


(5.8) 


the  drive  behaves  like  a  torque- controlled  generator  since,  in  view  of  the  large 
value  of  Gv,  the  driving  torque  is  practically  independent  of  the  angular  ve¬ 
locity. 

As  regards  the  dynamic  behaviour,  it  is  worth  considering  a  reduced-order 
model  which  can  be  obtained  by  neglecting  the  electric  time  constant  La/Ra 
with  respect  to  the  mechanical  time  constant  Im/Fm,  assuming  Tv  «  0  and 
a  purely  proportional  controller.  These  assumptions,  together  with  ki  =  0, 
lead  to  the  block  scheme  in  Fig.  5.3  for  the  velocity-controlled  generator.  On 
the  other  hand,  if  it  is  assumed  Kki  ^  Ra  and  kvfi/Kki  «  0,  the  resulting 
block  scheme  of  the  torque-controlled  generator  is  that  in  Fig.  5.4.  From  the 


1  It  is  assumed  Ci(0)  =  1;  in  the  case  of  presence  of  an  integral  action  in  Ci(s),  it 
should  be  lims^o  sC(s)  =  1. 
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Fig.  5.4.  Block  scheme  of  an  electric  drive  as  a  torque-controlled  generator 


above  schemes,  the  following  input/output  relations  between  control  voltage, 
reaction  torque,  and  angular  velocity  can  be  derived: 
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for  the  velocity-controlled  generator,  and 
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(5.9) 


(5.10) 


for  the  torque-controlled  generator.  These  transfer  functions  show  how,  with¬ 
out  current  feedback,  the  system  has  a  better  rejection  of  disturbance  torques 
in  terms  of  both  equivalent  gain  ( Ra/kvkt  -C  1  /Fm)  and  time  response 
(  R'O  krn  /  k  v  kf  I'rri  /  Fjyi ) . 

The  relationship  between  the  control  input  and  the  actuator  position  out¬ 
put  can  be  expressed  in  a  unified  manner  by  the  transfer  function 
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for  the  velocity-controlled  generator,  while  for  the  torque-controlled  generator 
it  is 


k 


m 


(5.13) 


Notice  how  the  power  amplifier,  in  the  velocity  control  case,  contributes  to 
the  input/output  relation  with  the  constant  Gv,  while  in  the  case  of  current 
control  the  amplifier,  being  inside  a  local  feedback  loop,  does  not  appear  as  a 
stand  alone  but  rather  in  the  expression  of  km  with  a  factor  1  / fc, . 

These  considerations  lead  to  the  following  conclusions.  In  all  such  appli¬ 
cations  where  the  drive  system  has  to  provide  high  rejection  of  disturbance 
torques  (as  in  the  case  of  independent  joint  control,  see  Sect.  8.3)  it  is  not 
advisable  to  have  a  current  feedback  in  the  loop,  at  least  when  all  quantities 
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Fig.  5.5.  Block  scheme  of  an  electric  drive  with  nonlinear  current  feedback 


are  within  their  nominal  values.  In  this  case,  the  problem  of  setting  a  pro¬ 
tection  can  be  solved  by  introducing  a  current  limit  that  is  not  performed  by 
a  saturation  on  the  control  signal  but  it  exploits  a  current  feedback  with  a 
dead-zone  nonlinearity  on  the  feedback  path,  as  shown  in  Fig.  5.5.  Therefore, 
an  actual  current  limit  is  obtained  whose  precision  is  as  high  as  the  slope 
of  the  dead  zone;  it  is  understood  that  stability  of  the  current  loop  is  to  be 
addressed  when  operating  in  this  way. 

As  will  be  shown  in  Sect.  8.5,  centralized  control  schemes,  instead,  demand 
the  drive  system  to  behave  as  a  torque-controlled  generator.  It  is  then  clear 
that  a  current  feedback  with  a  suitable  regulator  C)(,s)  should  be  used  so  as 
to  confer  a  good  static  and  dynamic  behaviour  to  the  current  loop.  In  this 
case,  servoing  of  the  driving  torque  is  achieved  indirectly,  since  it  is  based  on  a 
current  measurement  which  is  related  to  the  driving  torque  by  means  of  gain 
i  At- 


5.2.2  Hydraulic  Drives 

No  matter  how  a  hydraulic  servomotor  is  constructed,  the  derivation  of  its 
input/output  mathematical  model  refers  to  the  basic  equations  describing 
the  relationship  between  flow  rate  and  pressure,  the  relationship  between  the 
fluid  and  the  parts  in  motion,  and  the  mechanical  balance  of  the  parts  in 
motion.  Let  Q  represent  the  volume  flow  rate  supplied  by  the  distributor;  the 
flow  rate  balance  is  given  by  the  equation 

Q  =  Qm  +  Qi  +  Qc  (5-14) 

where  Qm  is  the  flow  rate  transferred  to  the  motor,  Qi  is  the  flow  rate  due  to 
leakage,  and  Qc  is  the  flow  rate  related  to  fluid  compressibility.  The  terms  Qi 
and  Qc  are  taken  into  account  in  view  of  the  high  operating  pressures  (of  the 
order  of  100  atm). 

Let  P  denote  the  differential  pressure  of  the  servomotor  due  to  the  load; 
then  it  can  be  assumed  that 

Qi  =  hP.  (5.15) 

Regarding  the  loss  for  compressibility,  if  V  denotes  the  instantaneous  volume 
of  the  fluid,  one  has 


Qc  =  iVsP 


(5.16) 
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Fig.  5.6.  Block  scheme  of  a  hydraulic  drive 


where  7  is  the  uniform  compressibility  coefficient  of  the  fluid.  Notice  that  the 
proportional  factor  kc  =  jV  between  the  time  derivative  of  the  pressure  and 
the  flow  rate  due  to  compressibility  depends  on  the  volume  of  the  fluid;  there¬ 
fore,  in  the  case  of  rotary  servomotors,  kc  is  a  constant,  whereas  in  the  case 
of  a  linear  servomotor,  the  volume  of  fluid  varies  and  thus  the  characteristic 
of  the  response  depends  on  the  operating  point. 

The  volume  flow  rate  transferred  to  the  motor  is  proportional  to  the  vol¬ 
ume  variation  in  the  chambers  per  time  unit;  with  reference  from  now  on  to  a 
rotary  servomotor,  such  variation  is  proportional  to  the  angular  velocity,  and 
then 

Qm,  =  kqf2m.  (5.17) 

The  mechanical  balance  of  the  parts  in  motion  is  described  by 

Cm  =  (slm  +  Fm)f2m  +  Cl  (5.18) 

with  obvious  meaning  of  the  symbols.  Finally,  the  driving  torque  is  propor¬ 
tional  to  the  differential  pressure  of  the  servomotor  due  to  the  load,  i.e., 


Cm  =  ktp. 


(5.19) 


Concerning  the  servovalve,  the  transfer  function  between  the  stem  position 
X  and  the  control  voltage  Vc  is  expressed  by 


X  =  Gs 

Vc  1  +  sTs 


(5.20) 


thanks  to  the  linearizing  effect  achieved  by  position  feedback;  Gs  is  the  equiv¬ 
alent  gain  of  the  servovalve,  whereas  its  time  constant  Ts  is  of  the  order  of  ms 
and  thus  it  can  be  neglected  with  respect  to  the  other  time  constants  of  the 
system. 

Finally,  regarding  the  distributor,  the  relationship  between  the  differen¬ 
tial  pressure,  the  flow  rate,  and  the  stem  displacement  is  highly  nonlinear; 
linearization  about  an  operating  point  leads  to  the  equation 


P  =  kxX  —  krQ. 


(5.21) 
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Fig.  5.7.  Schematic  representation  of  a  mechanical  gear 


By  virtue  of  (5.14)-(5.21),  the  servovalve/distributor/motor  complex  ( hy¬ 
draulic  drive )  is  represented  by  the  block  scheme  of  Fig.  5.6.  A  comparison 
between  the  schemes  in  Figs.  5.2  and  5.6  clearly  shows  the  formal  analogy 
in  the  dynamic  behaviour  of  an  electric  and  a  hydraulic  servomotor.  Never¬ 
theless,  such  analogy  should  not  induce  one  to  believe  that  it  is  possible  to 
make  a  hydraulic  drive  play  the  role  of  a  velocity-  or  torque-controlled  gener¬ 
ator,  as  for  an  electric  drive.  In  this  case,  the  pressure  feedback  loop  (formally 
analogous  to  the  current  feedback  loop)  is  indeed  a  structural  characteristic 
of  the  system  and,  as  such,  it  cannot  be  modified  but  with  the  introduction 
of  suitable  transducers  and  the  realization  of  the  relative  control  circuitry. 

5.2.3  Transmission  Effects 

In  order  to  describe  quantitatively  the  effects  introduced  by  the  use  of  a  trans¬ 
mission  ( mechanical  gear)  between  the  servomotor  and  the  actuated  joint,  it 
is  worth  referring  to  the  mechanical  coupling  realized  by  a  pair  of  spur  gears  of 
radius  rm  and  r,  which  is  schematically  represented  in  Fig.  5.7;  the  kinematic 
pair  is  assumed  to  be  ideal  (without  backlash)  and  connects  the  rotation  axis 
of  the  servomotor  with  the  axis  of  the  corresponding  joint. 

With  reference  to  an  electric  servomotor,  it  is  assumed  that  the  rotor  of 
the  servomotor  is  characterized  by  an  inertia  moment  Im  about  its  rotation 
axis  and  a  viscous  friction  coefficient  Fm ;  likewise,  I  and  F  denote  the  inertia 
moment  and  the  viscous  friction  coefficient  of  the  load.  The  inertia  moments 
and  the  friction  coefficients  of  the  gears  are  assumed  to  have  been  included 
in  the  corresponding  parameters  of  the  motor  (for  the  gear  of  radius  rm)  and 
of  the  load  (for  the  gear  of  radius  r).  Let  cm  denote  the  driving  torque  of  the 
motor  and  q  the  reaction  torque  applied  to  the  load  axis.  Also  let  and  dm 
denote  the  angular  velocity  and  position  of  the  motor  axis,  while  w  and  i9 
denote  the  corresponding  quantities  at  the  load  side.  Finally,  /  indicates  the 
force  exchanged  at  the  contact  between  the  teeth  of  the  two  gears.2 

2  In  the  case  considered,  it  has  been  assumed  that  both  the  motor  and  the  load 
are  characterized  by  revolute  motions;  if  the  load  should  exhibit  a  translation 
motion,  the  following  arguments  can  be  easily  extended,  with  analogous  results, 
by  replacing  the  angular  displacements  with  linear  displacements  and  the  inertia 
moments  with  masses  at  the  load  side. 
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The  gear  reduction  ratio  is  defined  as 


kr 


U! 


(5.22) 


since,  in  the  absence  of  slipping  in  the  kinematic  coupling,  it  is  rmftm  =  rft. 

The  gear  reduction  ratio,  in  the  case  when  it  is  representative  of  the  cou¬ 
pling  between  a  servomotor  and  the  joint  of  a  robot  manipulator,  attains 
values  much  larger  than  unity  (rm  <r)  —  typically  from  a  few  tens  to  a  few 
hundreds. 

The  force  /  exchanged  between  the  two  gears  generates  a  reaction  torque  /• 
rm  for  the  motion  at  the  motor  axis  and  a  driving  torque  /  •  r  for  the  rotation 
motion  of  the  load. 

The  mechanical  balances  at  the  motor  side  and  the  load  side  are  respec¬ 
tively: 


Cm  /m  T  k\r!jjrn  +  fr  m  (5.23) 

fr  =  Ito  +  Fuj  +  Ci.  (5.24) 


To  describe  the  motion  with  reference  to  the  motor  angular  velocity,  in  view 
of  (5.22),  combining  the  two  equations  gives  at  the  motor  side 


Cl 


—  leq&m  T  F^LOm  1“  . 


(5.25) 


where 


p  — 
±  eq  — 


(5.26) 


The  expressions  (5.25),  (5.26)  show  how,  in  the  case  of  a  gear  with  large 
reduction  ratio,  the  inertia  moment  and  the  viscous  friction  coefficient  of  the 
load  are  reflected  at  the  motor  axis  with  a  reduction  of  a  factor  1  /k‘2r:;  the 
reaction  torque,  instead,  is  reduced  by  a  factor  l/kr.  If  this  torque  depends 
on  ft  in  a  nonlinear  fashion,  then  the  presence  of  a  large  reduction  ratio  tends 
to  linearize  the  dynamic  equation. 


Example  5.1 

In  Fig.  5.8  a  rigid  pendulum  is  represented,  which  is  actuated  by  the  torque  f  ■  r  to 
the  load  axis  after  the  gear.  In  this  case,  the  dynamic  equations  of  the  system  are 

Cm  —  kmOOm  T  FmiOm  +  fr  m  (5.27) 

fr  =  Iio  +  Fu>  +  mglsm  ft  (5.28) 


where  I  is  the  inertia  moment  of  the  pendulum  at  the  load  axis,  F  is  the  viscous  fric¬ 
tion  coefficient,  m  is  the  pendulum  mass,  £  its  length  and  g  the  gravity  acceleration. 
Reporting  (5.28)  to  the  motor  axis  gives 

_  r  •  ,  ip  ,  ( m9?\  ■  (®rn 

Cm  —  leqOJm  T  FeqiOm  T  (  ,  j  Sm  I 

\  Kr  /  V  Kr 


(5.29) 
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Fig.  5.8.  Pendulum  actuated  via  mechanical  gear 


from  which  it  is  clear  how  the  contribution  of  the  nonlinear  term  is  reduced  by  the 
factor  kr- 


The  example  of  the  pendulum  has  been  considered  to  represent  an  n-link 
manipulator  with  revolute  joints,  for  which  each  link,  considered  as  isolated 
from  the  others,  can  be  considered  as  a  simple  rigid  pendulum.  The  connection 
with  other  links  introduces,  in  reality,  other  nonlinear  effects  which  complicate 
the  input/output  model;  in  this  regard,  it  is  sufficient  to  notice  that,  in  the 
case  of  a  double  pendulum,  the  inertia  moment  at  the  motor  side  of  the  first 
link  depends  also  on  the  angular  position  of  the  second  link. 

In  Chap.  7  the  effect  introduced  by  the  presence  of  transmissions  in  a 
generic  n-link  manipulator  structure  will  be  studied  in  detail.  Nevertheless,  it 
can  already  be  understood  how  the  nonlinear  couplings  between  the  motors  of 
the  various  links  will  be  reduced  by  the  presence  of  transmissions  with  large 
reduction  ratios. 


5.2.4  Position  Control 

After  having  examined  the  modalities  to  control  the  angular  velocity  of  an 
electric  or  hydraulic  drive,  the  motion  control  problem  for  a  link  of  a  generic 
manipulator  is  to  be  solved.  A  structure  is  sought  which  must  be  capable  of 
determining,  in  an  automatic  way,  the  time  evolution  of  the  quantity  chosen 
to  control  the  drive,  so  that  the  actuated  joint  executes  the  required  motion 
allowing  the  end-effector  to  execute  a  given  task. 

Once  a  trajectory  has  been  specified  for  the  end-effector  pose,  the  solu¬ 
tion  of  the  inverse  kinematics  problem  allows  the  computation  of  the  desired 
trajectories  for  the  various  joints,  which  thus  can  be  considered  as  available. 

Several  control  techniques  can  be  adopted  to  control  the  manipulator  mo¬ 
tion;  the  choice  of  a  particular  solution  depends  on  the  required  dynamic 
performance,  the  kind  of  motion  to  execute,  the  kinematic  structure,  and  the 
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Fig.  5.9.  General  block  scheme  of  electric  drive  control 


choice  to  utilize  either  servomotors  with  transmissions  or  torque  motors  with 
joint  direct  drive. 

The  simplest  solution  is  to  consider,  at  first  approximation,  the  motion 
of  a  joint  independent  of  the  motion  of  the  other  joints,  i.e.,  the  interaction 
can  be  regarded  as  a  disturbance.  Assume  the  reference  trajectory  $r(i)  is 
available.  According  to  classical  automatic  control  theory,  to  ensure  that  the 
angular  motor  position  i9m,  properly  measured  by  means  of  a  transducer  with 
constant  kxp,  follows  $r,  it  is  worth  resorting  to  a  feedback  control  system 
providing  ‘robustness’  with  respect  to  both  model  uncertainty  on  the  motor 
and  the  load,  and  the  presence  of  a  disturbance.  A  more  detailed  treatment 
is  deferred  to  Chap.  8,  where  the  most  congenial  solutions  to  solve  the  above 
problems  will  be  presented. 

In  the  following,  the  problem  of  joint  position  control  is  tackled  by  assum¬ 
ing  an  electric  DC  servomotor;  the  choice  is  motivated  by  the  diffusion  of  this 
technology,  due  to  the  high  flexibility  of  these  actuators  providing  optimal 
responses  in  the  large  majority  of  motion  control  applications. 

The  choice  of  a  feedback  control  system  to  realize  a  position  servo  at 
the  motor  axis  requires  the  adoption  of  a  controller ;  this  device  generates 
a  signal  which,  applied  to  the  power  amplifier,  automatically  generates  the 
driving  torque  producing  an  axis  motion  very  close  to  the  desired  motion  'dr. 
Its  structure  should  be  so  that  the  error  between  the  reference  input  and  the 
measured  output  is  minimized,  even  in  the  case  of  inaccurate  knowledge  of 
the  dynamics  of  the  motor,  the  load,  and  a  disturbance.  The  rejection  action 
of  the  disturbance  is  the  more  efficient,  the  smaller  the  magnitude  of  the 
disturbance. 

On  the  other  hand,  according  to  (5.9),  the  disturbance  is  minimized,  pro¬ 
vided  the  drive  is  velocity-controlled.  In  this  case,  in  view  of  (5.6),  the  reaction 
torque  influences  the  motor  axis  velocity  with  a  coefficient  equal  to  Ra/kvkt 
which  is  much  smaller  than  1/Fm,  which  represents  instead  the  weight  on 
the  reaction  torque  in  the  case  when  the  drive  is  torque-controlled.  Therefore, 
with  reference  to  Fig.  5.3,  the  general  scheme  of  drive  control  with  position 
feedback  is  illustrated  in  Fig.  5.9,  where  the  disturbance  d  represents  the  load 
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Fig.  5.10.  Block  scheme  of  drive  control  with  position  feedback 


torque  and  the  value  of  the  power  amplifier  gain  has  been  included  in  the 
control  action. 

Besides  reducing  the  effects  of  the  disturbance  on  the  output,  the  structure 
of  the  controller  must  ensure  an  optimal  trade-off  between  the  stability  of  the 
feedback  control  system  and  the  capability  of  the  output  to  dynamically  track 
the  reference  with  a  reduced  error. 

The  reduction  of  the  disturbance  effects  on  the  output  can  be  achieved 
by  conferring  a  large  value  of  the  gain  before  the  point  of  intervention  of 
the  disturbance,  without  affecting  stability.  If,  at  steady  state  (i9r  =  cost, 
Ci  =  cost),  it  is  desired  to  cancel  the  disturbance  effect  on  the  output,  the 
controller  must  act  an  integral  action  on  the  error  given  by  the  difference 
between  dr  and  kpp'dm- 

The  above  requirements  suggest  the  use  of  a  simple  controller  with  an  in¬ 
tegral  and  a  proportional  action  on  the  error;  the  proportional  action  is  added 
to  realize  a  stabilizing  action,  which,  however,  cannot  confer  to  the  closed-loop 
system  a  damped  transient  response  with  a  sufficiently  short  sampling  time. 
This  behaviour  is  due  to  the  presence  of  a  double  pole  at  the  origin  of  the 
transfer  function  of  the  forward  path. 

The  resulting  control  scheme  is  illustrated  in  Fig.  5.10,  where  km  and  Tm 
are  respectively  the  voltage-to-velocity  gain  constant  and  the  characteristic 
time  constant  of  the  motor  in  (5.12).  The  parameters  of  the  controller  Kp 
and  Tp  should  be  keenly  chosen  so  as  to  ensure  stability  of  the  feedback 
control  system  and  obtain  a  good  dynamic  behaviour. 

To  improve  the  transient  response,  the  industrial  drives  employed  for  po¬ 
sition  servoing  may  also  include  a  local  feedback  loop  based  on  the  angular 
velocity  measurement  (tachometer  feedback).  The  general  scheme  with  po¬ 
sition  and  velocity  feedback  is  illustrated  in  Fig.  5.11;  besides  the  position 
transducer,  a  velocity  transducer  is  used  with  constant  kpvi  as  well  as  a  sim¬ 
ple  proportional  controller  with  gain  Kp.  With  the  adoption  of  the  tachometer 
feedback,  the  proportional-integral  controller  with  parameters  Ky  and  Ty  is 
retained  in  the  internal  velocity  loop  so  as  to  cancel  the  effects  of  the  distur¬ 
bance  on  the  position  i9m  at  steady  state.  The  presence  of  two  feedback  loops, 
in  lieu  of  one,  around  the  intervention  point  of  the  disturbance  is  expected 
to  lead  to  a  further  reduction  of  the  disturbance  effects  on  the  output  also 
during  the  transients. 
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Fig.  5.11.  Block  scheme  of  drive  control  with  position  and  velocity  feedback 


The  adoption  of  tachometer  feedback  may  also  improve  the  transient  re¬ 
sponse  of  the  whole  control  system  with  respect  to  the  previous  case.  With 
a  keen  choice  of  the  controller  parameters,  indeed,  it  is  possible  to  achieve  a 
transfer  function  between  and  with  a  larger  bandwidth  and  reduced 
resonance  phenomena.  The  result  is  a  faster  transient  response  with  reduced 
oscillations,  thus  improving  the  capability  of  to  track  more  demanding 

reference  trajectories  i9r(t). 

The  above  analysis  will  be  further  detailed  in  Sect.  8.3. 

The  position  servo  may  also  utilize  a  current-controller  motor;  the  schemes 
in  Figs.  5.9-5.11  can  be  adopted,  provided  that  the  constants  in  (5.13)  are 
used  in  the  transfer  function  (5.11)  and  the  disturbance  D  is  weighed  with 
the  quantity  ki/kt  in  lieu  of  Ra/kt ■  In  that  case,  the  voltage  gain  Gv  of  the 
power  amplifier  will  not  contribute  to  the  control  action. 

As  a  final  consideration,  the  general  control  structure  presented  above 
may  be  extended  to  the  case  when  the  motor  is  coupled  to  a  load  via  a  gear 
reduction.  In  such  a  case,  it  is  sufficient  to  account  for  (5.25)  and  (5.26),  i.e. , 
replace  Im  and  Fm  with  the  quantities  Ieq  and  Feq,  and  scale  the  disturbance 
by  the  factor  1  /kr. 


5.3  Proprioceptive  Sensors 

The  adoption  of  sensors  is  of  crucial  importance  to  achieve  high-performance 
robotic  systems.  It  is  worth  classifying  sensors  into  proprioceptive  sensors  that 
measure  the  internal  state  of  the  manipulator,  and  exteroceptive  sensors  that 
provide  the  robot  with  knowledge  of  the  surrounding  environment. 

In  order  to  guarantee  that  a  coordinated  motion  of  the  mechanical  struc¬ 
ture  is  obtained  in  correspondence  of  the  task  planning,  suitable  parameter 
identification  and  control  algorithms  are  used  which  require  the  on-line  mea¬ 
surement,  by  means  of  proprioceptive  sensors,  of  the  quantities  characterizing 
the  internal  state  of  the  manipulator,  i.e.: 
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•  joint  positions, 

•  joint  velocities, 

•  joint  torques. 

On  the  other  hand,  typical  exteroceptive  sensors  include: 

•  force  sensors, 

•  tactile  sensors, 

•  proximity  sensors, 

•  range  sensors, 

•  vision  sensors. 

The  goal  of  such  sensors  is  to  extract  the  features  characterizing  the  in¬ 
teraction  of  the  robot  with  the  objects  in  the  environment,  so  as  to  enhance 
the  degree  of  autonomy  of  the  system.  To  this  class  also  belong  those  sensors 
which  are  specific  for  the  robotic  application,  such  as  sound,  humidity,  smoke, 
pressure,  and  temperature  sensors.  Fusion  of  the  available  sensory  data  can 
be  used  for  (high-level)  task  planning,  which  in  turn  characterizes  a  robot  as 
the  intelligent  connection  of  perception  to  action. 

In  the  following,  the  main  features  of  the  proprioceptive  sensors  are  illus¬ 
trated,  while  those  of  the  exteroceptive  sensors  will  be  presented  in  the  next 
section. 

5.3.1  Position  Transducers 

The  aim  of  position  transducers  is  to  provide  an  electric  signal  proportional 
to  the  linear  or  angular  displacement  of  a  mechanical  apparatus  with  respect 
to  a  given  reference  position.  They  are  mostly  utilized  for  control  of  machine 
tools,  and  thus  their  range  is  wide.  Potentiometers,  linear  variable-differential 
transformers  (LVDT),  and  inductosyns  may  be  used  to  measure  linear  dis¬ 
placements.  Potentiometers,  encoders,  resolvers  and  synchros  may  be  used  to 
measure  angular  displacements. 

Angular  displacement  transducers  are  typically  employed  in  robotics  ap¬ 
plications  since,  also  for  prismatic  joints,  the  servomotor  is  of  a  rotary  type. 
In  view  of  their  precision,  robustness  and  reliability,  the  most  common  trans¬ 
ducers  are  the  encoders  and  resolvers,  whose  operating  principles  are  detailed 
in  what  follows. 

On  the  other  hand,  linear  displacement  transducers  (LVDT’s  and  induc¬ 
tosyns)  are  mainly  employed  in  measuring  robots. 

Encoder 

There  are  two  types  of  encoder:  absolute  and  incremental.  The  absolute  en¬ 
coder  consists  of  an  optical-glass  disk  on  which  concentric  circles  (tracks)  are 
disposed;  each  track  has  an  alternating  sequence  of  transparent  sectors  and 
matte  sectors  obtained  by  deposit  of  a  metallic  film.  A  light  beam  is  emitted  in 
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Fig.  5.12.  Schematic  representation  of  an  absolute  encoder 


correspondence  of  each  track  which  is  intercepted  by  a  photodiode  or  a  photo¬ 
transistor  located  on  the  opposite  side  of  the  disk.  By  a  suitable  arrangement 
of  the  transparent  and  matte  sectors,  it  is  possible  to  convert  a  finite  number 
of  angular  positions  into  corresponding  digital  data.  The  number  of  tracks 
determines  the  length  of  the  word,  and  thus  the  resolution  of  the  encoder. 

To  avoid  problems  of  incorrect  measurement  in  correspondence  of  a  si¬ 
multaneous  multiple  transition  between  matte  and  transparent  sectors,  it  is 
worth  utilizing  a  Gray-code  encoder  whose  schematic  representation  is  given 
in  Fig.  5.12  with  reference  to  the  implementation  of  4  tracks  that  allow  the 
discrimination  of  16  angular  positions.  It  can  be  noticed  that  measurement 
ambiguity  is  eliminated,  since  only  one  change  of  contrast  occurs  at  each  tran¬ 
sition  (Table  5.1).  For  the  typical  resolution  required  for  joint  control,  absolute 
encoders  with  a  minimum  number  of  12  tracks  (bits)  are  employed  (resolution 
of  1/4096  per  circle).  Such  encoders  can  provide  unambiguous  measurements 
only  in  a  circle.  If  a  gear  reduction  is  present,  a  circle  at  the  joint  side  cor¬ 
responds  to  several  circles  at  the  motor  side,  and  thus  a  simple  electronics  is 
needed  to  count  and  store  the  number  of  actual  circles. 


Table  5.1.  Coding  table  with  Gray-code 


# 

Code 

# 

Code 

0 

0000 

8 

1100 

1 

0001 

9 

1101 

2 

0011 

10 

mi 

3 

0010 

11 

1110 

4 

0110 

12 

1010 

5 

0111 

13 

1011 

6 

0101 

14 

1001 

7 

0100 

15 

1000 
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Fig.  5.13.  Schematic  representation  of  an  incremental  encoder 


Incremental  encoders  have  a  wider  use  than  absolute  encoders,  since  they 
are  simpler  from  a  construction  viewpoint  and  thus  cheaper.  Like  the  absolute 
one,  the  incremental  encoder  consists  of  an  optical  disk  on  which  two  tracks 
are  disposed,  whose  transparent  and  matte  sectors  (in  equal  number  on  the 
two  tracks)  are  mutually  in  quadrature.  The  presence  of  two  tracks  also  allows, 
besides  the  number  of  transitions  associated  with  any  angular  rotation,  the 
detection  of  the  sign  of  rotation.  Often  a  third  track  is  present  with  one  single 
matte  sector  which  allows  the  definition  of  an  absolute  mechanical  zero  as 
a  reference  for  angular  position.  A  schematic  representation  is  illustrated  in 
Fig.  5.13. 

The  use  of  an  incremental  encoder  for  a  joint  actuating  system  clearly 
demands  the  evaluation  of  absolute  positions.  This  is  performed  by  means  of 
suitable  counting  and  storing  electronic  circuits.  To  this  end,  it  is  worth  notic¬ 
ing  that  the  position  information  is  available  on  volatile  memories,  and  thus 
it  can  be  corrupted  due  to  the  effect  of  disturbances  acting  on  the  electronic 
circuit,  or  else  fluctuations  in  the  supply  voltage.  Such  limitation  obviously 
does  not  occur  for  absolute  encoders,  since  the  angular  position  information 
is  coded  directly  on  the  optical  disk. 

The  optical  encoder  has  its  own  signal  processing  electronics  inside  the 
case,  which  provides  direct  digital  position  measurements  to  be  interfaced  with 
the  control  computer.  If  an  external  circuitry  is  employed,  velocity  measure¬ 
ments  can  be  reconstructed  from  position  measurements.  In  fact,  if  a  pulse 
is  generated  at  each  transition,  a  velocity  measurement  can  be  obtained  in 
three  possible  ways,  namely,  by  using  a  voltage-to-frequency  converter  (with 
analog  output),  by  (digitally)  measuring  the  frequency  of  the  pulse  train,  or 
by  (digitally)  measuring  the  sampling  time  of  the  pulse  train.  Between  these 
last  two  techniques,  the  former  is  suitable  for  high-speed  measurements  while 
the  latter  is  suitable  for  low-speed  measurements. 
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Fig.  5.14.  Electric  scheme  of  a  resolver  with  functional  diagram  of  a  tracking-type 
RDC 


Resolver 

The  resolver  is  an  electromechanical  position  transducer  which  is  compact 
and  robust.  Its  operating  principle  is  based  on  the  mutual  induction  between 
two  electric  circuits  which  allow  continuous  transmission  of  angular  position 
without  mechanical  limits.  The  information  on  the  angular  position  is  asso¬ 
ciated  with  the  magnitude  of  two  sinusoidal  voltages,  which  are  treated  by  a 
suitable  resolver-to-digital  converter  (RDC)  to  obtain  the  digital  data  corre¬ 
sponding  to  the  position  measurement.  The  electric  scheme  of  a  resolver  with 
the  functional  diagram  of  a  tracking-type  RDC  is  illustrated  in  Fig.  5.14. 

From  a  construction  viewpoint,  the  resolver  is  a  small  electric  machine 
with  a  rotor  and  a  stator;  the  inductance  coil  is  on  the  rotor  while  the  stator 
has  two  windings  at  90  electrical  degrees  one  from  the  other.  By  feeding  the 
rotor  with  a  sinusoidal  voltage  Rsinwf  (with  typical  frequencies  in  the  range 
of  0.4  to  10 kHz),  a  voltage  is  induced  on  the  stator  windings  whose  magni¬ 
tude  depends  on  the  rotation  angle  9.  The  two  voltages  are  fed  to  two  digital 
multipliers,  whose  input  is  a  and  whose  outputs  are  algebraically  summed 
to  achieve  Rsinwf  sin  (0  —  a);  this  signal  is  then  amplified  and  sent  to  the 
input  of  a  synchronous  detector,  whose  filtered  output  is  proportional  to  the 
quantity  sin  (9  —  a).  The  resulting  signal,  after  a  suitable  compensating  ac¬ 
tion,  is  integrated  and  then  sent  to  the  input  of  a  voltage-controlled  oscillator 
(VCO)  (a  voltage-to-frequency  converter)  whose  output  pulses  are  input  to  a 
forward-backward  counter.  Digital  data  of  the  quantity  a  are  available  on  the 
output  register  of  the  counter,  which  represent  a  measurement  of  the  angle  9. 

It  can  be  recognized  that  the  converter  works  according  to  a  feedback 
principle.  The  presence  of  two  integrators  (one  is  represented  by  the  forward- 
backward  counter)  in  the  loop  ensures  that  the  (digital)  position  and  (analog) 
velocity  measurements  are  error-free  as  long  as  the  rotor  rotates  at  constant 
speed;  actually,  a  round-off  error  occurs  on  the  word  a  and  thus  affects  the 
position  measurement.  The  compensating  action  is  needed  to  confer  suitable 
stability  properties  and  bandwidth  to  the  system.  Whenever  digital  data  are 
wished  also  for  velocity  measurements,  it  is  necessary  to  use  an  analog-to- 
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digital  converter.  Since  the  resolver  is  a  very  precise  transducer,  a  resolution 
of  1  bit  out  of  16  can  be  obtained  at  the  output  of  the  RDC. 

5.3.2  Velocity  Transducers 

Even  though  velocity  measurements  can  be  reconstructed  from  position  trans¬ 
ducers,  it  is  often  preferred  to  resort  to  direct  measurements  of  velocity,  by 
means  of  suitable  transducers.  Velocity  transducers  are  employed  in  a  wide 
number  of  applications  and  are  termed  tachometers .  The  most  common  de¬ 
vices  of  this  kind  are  based  on  the  operating  principles  of  electric  machines. 
The  two  basic  types  of  tachometers  are  the  direct- current  (DC)  tachometer 
and  the  alternating- current  (AC)  tachometer . 


DC  tachometer 

The  direct-current  tachometer  is  the  most  used  transducer  in  the  applications. 
It  is  a  small  DC  generator  whose  magnetic  field  is  provided  by  a  permanent 
magnet.  Special  care  is  paid  to  its  construction,  so  as  to  achieve  a  linear 
input/output  relationship  and  to  reduce  the  effects  of  magnetic  hysteresis 
and  temperature.  Since  the  field  flux  is  constant,  when  the  rotor  is  set  in 
rotation,  its  output  voltage  is  proportional  to  angular  speed  according  to  the 
constant  characteristic  of  the  machine. 

Because  of  the  presence  of  a  commutator,  the  output  voltage  has  a  resid¬ 
ual  ripple  which  cannot  be  eliminated  by  proper  filtering,  since  its  frequency 
depends  on  angular  speed.  A  linearity  range  of  0.1  to  1%  can  be  obtained, 
whereas  the  residual  ripple  coefficient  is  of  2  to  5%  of  the  mean  value  of  the 
output  signal. 


AC  tachometer 

In  order  to  avoid  the  drawbacks  caused  by  the  presence  of  a  residual  ripple  in 
the  output  of  a  DC  tachometer,  one  may  resort  to  an  AC  tachometer.  While 
the  DC  tachometer  is  a  true  DC  generator,  the  AC  tachometer  differs  from  a 
generator.  In  fact,  if  a  synchronous  generator  would  be  used,  the  frequency  of 
the  output  signal  would  be  proportional  to  the  angular  speed. 

To  obtain  an  alternating  voltage  whose  magnitude  is  proportional  to  speed, 
one  may  resort  to  an  electric  machine  that  is  structurally  different  from  the 
synchronous  generator.  The  AC  tachometer  has  two  windings  on  the  stator 
mutually  in  quadrature  and  a  cup  rotor.  If  one  of  the  windings  is  fed  by  a 
constant-magnitude  sinusoidal  voltage,  a  sinusoidal  voltage  is  induced  on  the 
other  winding  which  has  the  same  frequency,  a  magnitude  proportional  to 
angular  speed,  and  a  phase  equal  or  opposite  to  that  of  the  input  voltage 
according  to  the  sign  of  rotation;  the  exciting  frequency  is  usually  set  to 
400  Hz.  The  use  of  a  synchronous  detector  then  yields  an  analog  measurement 
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Fig.  5.15.  a)  Schematic  representation  of  a  strain  gauge,  b)  Its  insertion  in  a 
Wheatstone  bridge 

of  angular  velocity.  In  this  case,  the  output  ripple  can  be  eliminated  by  a 
proper  filter,  since  its  fundamental  frequency  is  twice  as  much  as  the  supply 
frequency. 

The  performance  of  AC  tachometers  is  comparable  to  that  of  DC  tachome¬ 
ters.  Two  further  advantages  of  AC  tachometers  are  the  lack  of  wiping  contacts 
and  the  presence  of  a  low  moment  of  inertia,  in  view  of  the  use  of  a  lightweight 
cup  rotor.  However,  a  residual  voltage  occurs,  even  when  the  rotor  is  still,  be¬ 
cause  of  the  unavoidable  parasitic  couplings  between  the  stator  coil  and  the 
measurement  circuitry. 


5.4  Exteroceptive  Sensors 

5.4.1  Force  Sensors 

Measurement  of  a  force  or  torque  is  usually  reduced  to  measurement  of  the 
strain  induced  by  the  force  (torque)  applied  to  an  extensible  element  of  suit¬ 
able  features.  Therefore,  an  indirect  measurement  of  force  is  obtained  by 
means  of  measurements  of  small  displacements.  The  basic  component  of  a 
force  sensor  is  the  strain  gauge  which  uses  the  change  of  electric  resistance  of 
a  wire  under  strain. 


Strain  gauge 

The  strain  gauge  consists  of  a  wire  of  low  temperature  coefficient.  The  wire  is 
disposed  on  an  insulated  support  (Fig.  5.15a)  which  is  glued  to  the  element 
subject  to  strain  under  the  action  of  a  stress.  Dimensions  of  the  wire  change 
and  then  they  cause  a  change  of  electric  resistance. 

The  strain  gauge  is  chosen  in  such  a  way  that  the  resistance  Rs  changes 
linearly  in  the  range  of  admissible  strain  for  the  extensible  element.  To  trans¬ 
form  changes  of  resistance  into  an  electric  signal,  the  strain  gauge  is  inserted  in 
one  arm  of  a  Wheatstone  bridge  which  is  balanced  in  the  absence  of  stress  on 
the  strain  gauge  itself.  From  Fig.  5.15b  it  can  be  understood  that  the  voltage 
balance  in  the  bridge  is  described  by 
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If  temperature  variations  occur,  the  wire  changes  its  dimension  without 
application  of  any  external  stress.  To  reduce  the  effect  of  temperature  varia¬ 
tions  on  the  measurement  output,  it  is  worth  inserting  another  strain  gauge 
in  an  adjacent  arm  of  the  bridge,  which  is  glued  on  a  portion  of  the  extensible 
element  not  subject  to  strain. 

Finally,  to  increase  bridge  sensitivity,  two  strain  gauges  may  be  used  which 
have  to  be  glued  on  the  extensible  element  in  such  a  way  that  one  strain  gauge 
is  subject  to  traction  and  the  other  to  compression;  the  two  strain  gauges  then 
have  to  be  inserted  in  two  adjacent  arms  of  the  bridge. 

Shaft  torque  sensor 

In  order  to  employ  a  servomotor  as  a  torque-controlled  generator,  an  indirect 
measurement  of  the  driving  torque  is  typically  used,  e.g.,  through  the  mea¬ 
surement  of  armature  current  in  a  permanent-magnet  DC  servomotor.  If  it  is 
desired  to  guarantee  insensitivity  to  change  of  parameters  relating  torque  to 
the  measured  physical  quantities,  it  is  necessary  to  resort  to  a  direct  torque 
measurement . 

The  torque  delivered  by  the  servomotor  to  the  joint  can  be  measured  by 
strain  gauges  mounted  on  an  extensible  apparatus  interposed  between  the 
motor  and  the  joint,  e.g.,  a  hollow  shafting.  Such  apparatus  must  have  low 
torsional  stiffness  and  high  bending  stiffness,  and  it  must  ensure  a  proportional 
relationship  between  the  applied  torque  and  the  induced  strain. 

By  connecting  the  strain  gauges  mounted  on  the  hollow  shafting  (in  a 
Wheatstone  bridge  configuration)  to  a  slip  ring  by  means  of  graphite  brushes, 
it  is  possible  to  feed  the  bridge  and  measure  the  resulting  unbalanced  signal 
which  is  proportional  to  the  applied  torque. 

The  measured  torque  is  that  delivered  by  the  servomotor  to  the  joint,  and 
thus  it  does  not  coincide  with  the  driving  torque  Cm  in  the  block  schemes  of 
the  actuating  systems  in  Fig.  5.2  and  in  Fig.  5.6.  In  fact,  such  measurement 
does  not  account  for  the  inertial  and  friction  torque  contributions  as  well  as 
for  the  transmission  located  upstream  of  the  measurement  point. 


Wrist  force  sensor 

When  the  manipulator’s  end-effector  is  in  contact  with  the  working  environ¬ 
ment,  the  force  sensor  allows  the  measurement  of  the  three  components  of  a 
force  and  the  three  components  of  a  moment  with  respect  to  a  frame  attached 
to  it. 

As  illustrated  in  Fig.  5.16,  the  sensor  is  employed  as  a  connecting  apparatus 
at  the  wrist  between  the  outer  link  of  the  manipulator  and  the  end-effector. 
The  connection  is  made  by  means  of  a  suitable  number  of  extensible  elements 
subject  to  strain  under  the  action  of  a  force  and  a  moment.  Strain  gauges 
are  glued  on  each  element  which  provide  strain  measurements.  The  elements 
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Fig.  5.16.  Use  of  a  force  sensor  on  the  outer  link  of  a  manipulator 


have  to  be  disposed  in  a  keen  way  so  that  at  least  one  element  is  appreciably 
deformed  for  any  possible  orientation  of  forces  and  moments. 

Furthermore,  the  single  force  component  with  respect  to  the  frame  at¬ 
tached  to  the  sensor  should  induce  the  least  possible  number  of  deformations, 
so  as  to  obtain  good  structural  decoupling  of  force  components.  Since  a  com¬ 
plete  decoupling  cannot  be  achieved,  the  number  of  significant  deformations 
to  reconstruct  the  six  components  of  the  force  and  moment  vector  is  greater 
than  six. 

A  typical  force  sensor  is  that  where  the  extensible  elements  are  disposed  as 
in  a  Maltese  cross;  this  is  schematically  indicated  in  Fig.  5.17.  The  elements 
connecting  the  outer  link  with  the  end-effector  are  four  bars  with  a  rectangular 
parallelepiped  shape.  On  the  opposite  sides  of  each  bar,  a  pair  of  strain  gauges 
is  glued  that  constitute  two  arms  of  a  Wheatstone  bridge;  there  is  a  total  of 
eight  bridges  and  thus  the  possibility  of  measuring  eight  strains. 

The  matrix  relating  strain  measurements  to  the  force  components  ex¬ 
pressed  in  a  Frame  s  attached  to  the  sensor  is  termed  sensor  calibration  matrix. 
Let  uj,,  for  i  =  1, . . .  ,8,  denote  the  outputs  of  the  eight  bridges  providing  mea¬ 
surement  of  the  strains  induced  by  the  applied  forces  on  the  bars  according 
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Fig.  5.17.  Schematic  representation  of  a  Maltese-cross  force  sensor 


to  the  directions  specified  in  Fig.  5.17.  Then,  the  calibration  matrix  is  given 
by  the  transformation 


~Wi~ 

■  /I  1  0  0  C13  o  0  0  C17  0  1  U>2 

fy  C21  0  0  0  C25  0  0  0  W3 

fz  _  0  C32  0  C34  0  C36  0  C38  u>4 

fisx  0  0  0  C44  0  0  0  C48  w$ 

fisy  0  C52  0  0  0  C56  0  0  wq 

-flsz\  L  Cgi  0  C63  0  C(55  0  C67  0  J  W7 


(5.31) 


Reconstruction  of  force  measurements  through  the  calibration  matrix  is  en¬ 
trusted  to  suitable  signal  processing  circuitry  available  in  the  sensor. 

Typical  sensors  have  a  diameter  of  about  10  cm  and  a  height  of  about  5  cm, 
with  a  measurement  range  of  50  to  500  N  for  the  forces  and  of  5  to  70N-m  for 
the  torques,  and  a  resolution  of  the  order  of  0.1%  of  the  maximum  force  and 
of  0.05%  of  the  maximum  torque,  respectively;  the  sampling  frequency  at  the 
output  of  the  processing  circuitry  is  of  the  order  of  1kHz. 

Finally,  it  is  worth  noticing  that  force  sensor  measurements  cannot  be 
directly  used  by  a  force/motion  control  algorithm,  since  they  describe  the 
equivalent  forces  acting  on  the  sensors  which  differ  from  the  forces  applied  to 
the  manipulator’s  end-effector  (Fig.  5.16).  It  is  therefore  necessary  to  trans- 
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form  those  forces  from  the  sensor  Frame  s  into  the  constraint  Frame  c;  in  view 
of  the  transformation  in  (3.116),  one  has 


R-  o]\rs- 

_S(rccs)Rcs  Rcs\  [/ij. 


(5.32) 


which  requires  knowledge  of  the  position  of  the  origin  of  Frame  s  with 
respect  to  Frame  c  as  well  as  of  the  orientation  Rcs  of  Frame  s  with  respect 
to  Frame  c.  Both  such  quantities  are  expressed  in  Frame  c,  and  thus  they  are 
constant  only  if  the  end-effector  is  still,  once  contact  has  been  achieved. 


5.4.2  Range  Sensors 

The  primary  function  of  the  exteroceptive  sensors  is  to  provide  the  robot  with 
the  information  needed  to  execute  ‘intelligent’  actions  in  an  autonomous  way. 
To  this  end,  it  is  crucial  to  detect  the  presence  of  an  object  in  the  workspace 
and  eventually  to  measure  its  range  from  the  robot  along  a  given  direction. 

The  former  kind  of  data  is  provided  by  the  proximity  sensors,  a  simplified 
type  of  range  sensors,  capable  of  detecting  only  the  presence  of  objects  nearby 
the  sensitive  part  of  the  sensor,  without  a  physical  contact.  The  distance 
within  which  such  sensors  detect  objects  is  defined  sensitive  range. 

In  the  more  general  case,  range  sensors  are  capable  of  providing  structured 
data,  given  by  the  distance  of  the  measured  object  and  the  corresponding 
measurement  direction,  i.e. ,  the  position  in  space  of  the  detected  object  with 
respect  to  the  sensor. 

The  data  provided  by  the  range  sensors  are  used  in  robotics  to  avoid 
obstacles,  build  maps  of  the  environment,  recognize  objects. 

The  most  popular  range  sensors  in  robotics  applications  are  those  based 
on  sound  propagation  through  an  elastic  fluid,  the  so-called  sonars  (SOund 
NAvigation  and  Ranging),  and  those  exploiting  light  propagation  features,  the 
so-called  lasers  (Light  Amplification  by  Stimulated  Emission  of  Radiation). 
In  the  following,  the  main  features  of  these  two  sensors  are  illustrated. 


Sonars 

The  sonars  employ  acoustic  pulses  and  their  echoes  to  measure  the  range  to  an 
object.  Since  the  sound  speed  is  usually  known  for  a  given  media  (air,  water), 
the  range  to  an  object  is  proportional  to  the  echo  travel  time,  commonly  called 
time- of- flight ,  i.e.,  the  time  which  the  acoustic  wave  takes  to  cover  the  distance 
sensor-object-sensor.  Sonars  are  widely  utilized  in  robotics,  and  especially  in 
mobile  and  underwater  robotics.  Their  popularity  is  due  to  their  low  cost,  light 
weight,  low  power  consumption,  and  low  computational  effort,  compared  to 
other  ranging  sensors.  In  some  applications,  such  as  in  underwater  and  low- 
visibility  environments,  the  sonar  is  often  the  only  viable  sensing  modality. 

Despite  a  few  rare  examples  of  sonars  operating  at  audible  frequencies 
for  human  ears  (about  20 Hz  to  20 KHz),  the  ultrasound  frequencies  (higher 
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Fig.  5.18.  Sonar  ranging  principle 


than  20  KHz)  are  the  most  widely  used  to  realize  this  type  of  sensor.  Typical 
frequencies  in  robotics  range  from  20  KHz  to  200  KHz,  even  though  higher 
values  (of  the  order  of  MHz)  can  be  achieved  utilizing  piezoelectric  quartz 
crystals.  In  this  range,  the  energy  of  the  wave  emitted  by  the  sonar  can  be 
regarded  as  concentrated  in  a  conical  volume  whose  beamwidth  depends  on 
the  frequency  as  well  as  on  the  transducer  diameter.  Further  to  measuring 
range,  sonars  provide  qualitative  directional  data  on  the  object  which  has 
generated  the  echo.  For  the  most  common  sensors  in  robotics,  the  beamwidth 
of  the  energy  beam  is  typically  not  smaller  than  15  deg.  Obviously,  for  smaller 
beamwidths,  higher  angular  resolutions  can  be  obtained. 

The  main  components  of  a  sonar  measurement  system  are  a  transducer, 
which  is  vibrated  and  transforms  acoustic  energy  into  electric  energy  and  vice 
versa,  and  a  circuitry  for  the  excitation  of  the  transducer  and  the  detection  of 
the  reflected  signal.  Figure  5.18  schematically  illustrates  the  operating  prin¬ 
ciple:  the  pulse  I  emitted  by  the  transducer,  after  hitting  the  object  O  found 
in  the  emission  cone  of  the  sensor,  is  partly  reflected  (echo  E)  towards  the 
sound  source  and  thus  detected.  The  time-of- flight  tv  is  the  time  between  the 
emission  of  the  ultrasound  pulse  and  the  reception  of  the  echo.  The  object 
range  do  can  be  computed  from  tv  using  the  relation 

do  =  ^  (5.33) 

where  cs  is  sound  speed,  which  in  low-humidity  air  depends  on  the  tempera¬ 
ture  T  (measured  in  centigrade)  according  to  the  expression 

cs  «  20.05VT  + 273.16  m/s.  (5.34) 

In  the  scheme  of  Fig.  5.18  the  use  of  a  sole  transducer  is  represented  for 
the  transmission  of  the  pulse  and  the  reception  of  the  echo.  This  configura¬ 
tion  requires  that  the  commutation  from  transmitter  to  receiver  takes  place 
after  a  certain  latency  time  which  depends  not  only  on  the  duration  of  the 
transmitted  pulse  but  also  on  the  mechanical  inertia  of  the  transducer. 

Despite  the  low  cost  and  ease  of  use,  however,  these  sensors  have  non- 
negligible  limits  with  respect  to  the  angular  and  radial  resolution,  as  well  as 
to  the  minimum  and  maximum  measurement  range  that  can  be  achieved.  In 
particular,  the  width  of  the  radiation  cone  decreases  as  frequency  increases 
with  improved  angular  resolution.  A  higher  frequency  leads  to  greater  radial 
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Fig.  5.19.  Reflector  models  on  smooth  surfaces:  a)  non-detected  plane,  b)  non- 
detected  corner,  c)  plane  with  false  detection  ( O  real  object,  Ov  virtual  object 
detected) 


resolution  and  contributes  to  reducing  the  minimum  range  that  can  be  de¬ 
tected  by  the  sonar.  Nevertheless,  there  is  a  lower  limit  because  of  the  lapse 
time  when  reception  is  inhibited  to  avoid  interference  with  the  reflected  sig¬ 
nal  —  in  certain  cases  better  performance  can  be  obtained  by  employing  two 
distinct  transducers  for  the  emission  and  the  detection.  On  the  other  hand, 
too  high  frequencies  may  exasperate  absorbtion  phenomena,  depending  on  the 
features  of  the  surface  generating  the  echo.  Such  phenomena  further  reduce 
the  power  of  the  transmitted  signal  decreasing  with  the  square  of  the  range 
covered  by  the  ultrasound  wave  —  thus  reducing  the  maximum  limit  of  the 
measurement  time. 

Piezoelectric  and  electrostatic  transducers  are  the  two  major  types  avail¬ 
able  that  operate  in  air  and  can  in  principle  operate  both  as  a  transmitter 
and  receiver. 

The  piezoelectric  transducers  exploit  the  property  of  some  crystal  materials 
to  deform  under  the  action  of  an  electric  field  and  vibrate  when  a  voltage  is 
applied  at  the  resonant  frequency  of  the  crystal.  The  efficiency  of  the  acoustic 
match  of  these  transducers  with  compressible  fluids  such  as  air  is  rather  low. 
Often  a  conical  concave  horn  is  mounted  on  the  crystal  to  match  acoustically 
the  crystal  acoustic  impedance  to  that  of  air.  Being  of  resonant  type,  these 
transducers  are  characterized  by  a  rather  low  bandwidth  and  show  a  significant 
mechanical  inertia  which  severely  limits  the  minimum  detectable  range,  thus 
justifying  the  use  of  two  distinct  transducers  as  transmitter  and  receiver. 

The  electrostatic  transducers  operate  as  capacitors  whose  capacitance 
varies  moving  and/or  deforming  one  of  its  plates.  A  typical  construction  con¬ 
sists  of  a  gold-coated  plastic  foil  membrane  ( mobile  plate)  stretched  across  a 
round  grooved  aluminium  back  plate  ( fixed  plate).  When  the  transducer  op¬ 
erates  as  receiver,  the  change  of  capacitance,  induced  by  the  deformation  of 
the  membrane  under  the  acoustic  pressure,  produces  a  proportional  change  of 
the  voltage  across  the  capacitor,  assuming  that  the  foil  charge  is  constant.  As 
a  transmitter,  the  transducer  membrane  is  vibrated  by  applying  a  sequence 
of  electric  pulses  across  the  capacitor.  The  electric  oscillations  generate,  as 
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Fig.  5.20.  Time-of-flight  laser  sensor  operating  principle 


a  result  of  the  induced  electric  field,  a  mechanical  force  which  vibrates  the 
mobile  plate. 

Since  the  electrostatic  transducers  can  operate  at  different  frequencies, 
they  are  characterized  by  large  bandwidth  and  high  sensitivity,  low  mechan¬ 
ical  inertia  and  rather  efficient  acoustic  match  with  air.  As  compared  to  the 
piezoelectric  transducers,  however,  they  can  operate  at  lower  maximum  fre¬ 
quencies  (a  few  hundreds  kHz  vs  a  few  MHz)  and  require  a  bias  voltage  which 
complicates  the  control  electronics.  Among  the  ultrasound  measurement  sys¬ 
tems  with  capacitive  transducers,  it  is  worth  mentioning  the  Polaroid  sonar, 
initially  developed  for  autofocus  systems  and  later  widely  employed  as  range 
sensors  in  several  robotic  applications.  The  600  series  sensor  utilizes  a  ca¬ 
pacitive  transducer  of  the  type  described  above  with  a  diameter  of  almost 
4  cm,  operates  at  50  kHz  frequency  and  is  characterized  by  a  beamwidth  of 
15  deg,  can  detect  a  maximum  range  of  about  10  m  and  a  mimimum  range 
of  about  15  cm  with  an  accuracy  of  ±1%  across  the  measurement  range.  The 
bias  voltage  is  200  V  with  current  absorbtion  peaks  of  2  A  in  transmission. 

Accuracy  of  ultrasound  range  sensors  depends  on  the  features  of  the  trans¬ 
ducer  and  the  excitation/detection  circuitry,  as  well  as  on  the  reflective  prop¬ 
erties  of  the  surfaces  hit  by  the  acoustic  waves. 

Smooth  surfaces,  i.e.,  those  characterized  by  irregularities  of  comparable 
size  to  that  of  the  wavelength  corresponding  to  the  employed  frequency,  may 
produce  a  non-detectable  echo  at  the  sensor  (Figura  5.19a,b)  if  the  incident 
angle  of  the  ultrasound  beam  exceeds  a  given  critical  angle  which  depends 
on  the  operational  frequency  and  the  reflective  material.  In  the  case  of  the 
Polaroid  sensors,  this  angle  is  equal  to  65  deg,  i.e.,  25  deg  from  the  normal 
to  the  reflective  surface,  for  a  smooth  surface  in  plywood.  When  operating  in 
complex  environments,  such  mirror  reflections  may  give  rise  to  multiple  reflec¬ 
tions,  thus  causing  range  measurement  errors  or  false  detection  (Fig.  5.19c). 


Lasers 

In  the  construction  of  optical  measurement  systems,  the  laser  beam  is  usually 
preferred  to  other  light  sources  for  the  following  reasons: 
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•  They  can  easily  generate  bright  beams  with  lightweight  sources. 

•  The  infrared  beams  can  be  used  unobtrusively. 

•  They  focus  well  to  give  narrow  beams. 

•  Single-frequency  sources  allow  easier  rejection  filtering  of  unwanted  fre¬ 
quencies,  and  do  not  disperse  from  refraction  as  much  as  full  spectrum 

sources. 

There  are  two  types  of  laser-based  range  sensors  in  common  use:  the  time- 
of- flight  sensors  and  the  triangulation  sensors. 

The  time- of -flight  sensors  compute  distance  by  measuring  the  time  that  a 
pulse  of  light  takes  to  travel  from  the  source  to  the  observed  target  and  then  to 
the  detector  (usually  collocated  with  the  source).  The  travel  time  multiplied 
by  the  speed  of  light  (properly  adjusted  for  the  air  temperature)  gives  the 
distance  measurement.  The  operating  principle  of  a  time-of-flight  laser  sensor 
is  illustrated  in  Fig.  5.20. 

Limitations  on  the  accuracy  of  these  sensors  are  based  on  the  minimum 
observation  time  —  and  thus  the  minimum  distance  observable,  the  temporal 
accuracy  (or  quantization)  of  the  receiver,  and  the  temporal  width  of  the 
laser  pulse.  Such  limitations  are  not  only  of  a  technological  nature.  In  many 
cases,  cost  is  the  limiting  factor  of  these  measurement  devices.  For  instance,  to 
obtain  1  mm  resolution,  a  time  accuracy  of  about  3  ps,  which  can  be  achieved 
only  by  using  rather  expensive  technology. 

Many  time-of-flight  sensors  used  have  what  is  called  an  ambiguity  inter¬ 
val.  The  sensor  emits  pulses  of  light  periodically,  and  computes  an  average 
target  distance  from  the  time  of  the  returning  pulses.  Typically,  to  simplify 
the  detection  electronics  of  these  sensors,  the  receiver  only  accepts  signals 
that  arrive  within  time  At,  but  this  time  window  might  also  observe  previous 
pulses  reflected  by  more  distant  surfaces.  This  means  that  a  measurement  is 
ambiguous  to  the  multiple  of  \ cAt ,  where  c  is  the  speed  of  light.  Typical 
values  of  \cAt  are  20-40  m. 

In  certain  conditions,  suitable  algorithms  can  be  employed  to  recover  the 
true  depth  by  assuming  that  the  distances  should  be  changing  smoothly. 

The  time-of-flight  sensors  transmit  only  a  single  beam,  thus  range  mea¬ 
surements  are  only  obtained  from  a  single  surface  point.  In  order  to  obtain 
more  information,  the  range  data  is  usually  supplied  as  a  vector  of  range  to 
surfaces  lying  in  a  plane  or  as  an  image.  To  obtain  these  denser  representa¬ 
tions,  the  laser  beam  is  swept  across  the  scene.  Normally  the  beam  is  swept 
by  a  set  of  mirrors  rather  than  moving  the  laser  and  detector  themselves  — 
mirrors  are  lighter  and  less  prone  to  motion  damage. 

Typical  time-of-flight  sensors  suitable  for  mobile  robotics  applications  have 
a  range  of  5-100  m,  an  accuracy  of  5-10  mm,  and  a  frequency  of  data  acqui¬ 
sition  per  second  of  1000-25000  Hz. 
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Fig.  5.21.  Triangulation  laser  sensor  operating  principle 


The  operating  principle  of  triangulation  laser  sensors3  is  illustrated  in 
Fig.  5.21. 

The  laser  beam  emitted  by  a  photodiode  is  projected  onto  the  observed 
surface.  The  reflected  beam  is  focused  on  a  CCD  sensor  by  means  of  a  suitable 
lens.  Obviously,  reflection  must  be  diffused.  The  position  of  the  focused  beam 
reflected  to  the  receiver  gives  rise  to  a  signal  which  is  proportional  to  the 
distance  of  the  transmitter  from  the  object.  In  fact,  from  the  measurement 
of  the  CCD  sensor  it  is  possible  to  resort  to  the  angle  at  which  the  reflected 
energy  hits  the  sensor.  Once  the  relative  position  and  orientation  of  the  CCD 
sensor  with  respect  to  the  photodiode  are  known,  as  e.g.  through  a  suitable 
calibration  procedure,  it  is  possible  to  compute  the  distance  from  the  object 
with  simple  geometry. 

Accuracy  can  be  influenced  by  certain  object  surfaces  not  favouring  reflec¬ 
tion,  differences  or  changes  of  colour.  Such  occurrences  can  be  mitigated  or 
even  eliminated  with  modern  electronic  technology  and  automatic  regulation 
of  light  intensity. 

The  possibility  of  controlling  the  laser  beam  light  brings  the  following 
advantages: 


3  The  triangulation  method  is  based  on  the  trigonometric  properties  of  triangles 
and  in  particular  on  the  cosine  theorem.  The  method  allows  the  computation 
of  the  distance  between  two  non-directly  accessible  points,  i.e.,  once  two  angles 
and  one  side  of  a  triangle  are  known,  it  is  possible  to  determine  the  other  two 
sides.  For  the  case  at  issue,  one  side  is  given  by  the  distance  between  the  emitter 
(laser)  and  the  receiver  (the  CCD  sensor),  one  angle  is  given  by  the  orientation 
of  the  emitter  with  respect  to  that  side  and  the  other  angle  can  be  computed 
from  the  position  of  the  laser  beam  on  the  image  plane.  In  practice,  it  is  not  easy 
to  compute  the  above  quantities,  and  suitable  calibration  techniques  are  to  be 
employed  which  avoid  such  computation  to  determine  the  distance  measurement. 
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Fig.  5.22.  Schematic  representation  of  a  vision  system 


•  If  the  laser  beam  wavelength  is  known,  e.g.  that  of  the  visible  red  670  nm, 
highly  selective  filters  can  be  used  which  are  set  to  the  same  frequency  to 
reduce  the  effects  of  other  light  sources. 

•  The  laser  beam  may  be  remodelled  through  lenses  and  mirrors  so  as  to 
create  multiple  beams  or  laser  strips  to  measure  multiple  3D  points  simul¬ 
taneously. 

•  The  direction  of  the  laser  beam  can  be  controlled  directly  by  the  control 
system  to  observe  selectively  only  those  portions  of  the  scene  of  interest. 

The  main  limitations  of  this  type  of  sensors  are  the  potential  eye  safety 
risks  from  the  power  of  lasers,  particularly  when  invisible  laser  frequencies 
are  used  (commonly  infrared),  as  well  as  the  false  specular  reflections  from 
metallic  and  polished  objects. 


5.4.3  Vision  Sensors 

The  task  of  a  camera  as  a  vision  sensor  is  to  measure  the  intensity  of  the 
light  reflected  by  an  object.  To  this  end,  a  photosensitive  element,  termed 
pixel  (or  photosite) ,  is  employed,  which  is  capable  of  transforming  light  energy 
into  electric  energy.  Different  types  of  sensors  are  available  depending  on  the 
physical  principle  exploited  to  realize  the  energy  transformation.  The  most 
widely  used  devices  are  CCD  and  CMOS  sensors  based  on  the  photoelectric 
effect  of  semiconductors. 

CCD 

A  CCD  (Charge  Coupled  Device)  sensor  consists  of  a  rectangular  array  of  pho- 
tosites.  Due  to  the  photoelectric  effect,  when  a  photon  hits  the  semiconductor 
surface,  a  number  of  free  electrons  are  created,  so  that  each  element  accumu¬ 
lates  a  charge  depending  on  the  time  integral  of  the  incident  illumination  over 
the  photosensitive  element.  This  charge  is  then  passed  by  a  transport  mech¬ 
anism  (similar  to  an  analog  shift  register)  to  the  output  amplifier,  while  at 
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Fig.  5.23.  Perspective  transformation 


the  same  time  the  photosite  is  discharged.  The  electric  signal  is  to  be  further 
processed  in  order  to  produce  the  real  video  signal. 

CMOS 

A  CMOS  (Complementary  Metal  Oxide  Semiconductor)  sensor  consists  of 
a  rectangular  array  of  photodiodes.  The  junction  of  each  photodiode  is 
precharged  and  it  is  discharged  when  hit  by  photons.  An  amplifier  integrated 
in  each  pixel  can  transform  this  charge  into  a  voltage  or  current  level.  The 
main  difference  with  the  CCD  sensor  is  that  the  pixels  of  a  CMOS  sensor  are 
non-integrating  devices;  after  being  activated  they  measure  throughput,  not 
volume.  In  this  manner,  a  saturated  pixel  will  never  overflow  and  influence  a 
neighboring  pixel.  This  prevents  the  effect  of  blooming ,  which  indeed  affects 
CCD  sensors. 

Camera 

As  sketched  in  Fig.  5.22,  a  camera  is  a  complex  system  comprising  several 
devices  other  than  the  photosensitive  sensor,  i.e. ,  a  shutter,  a  lens  and  analog 
preprocessing  electronics .  The  lens  is  responsible  for  focusing  the  light  reflected 
by  the  object  on  the  plane  where  the  photosensitive  sensor  lies,  called  the 
image  plane. 

With  reference  to  Fig.  5.23,  consider  a  frame  Oc-xcyczc  attached  to  the 
camera,  whose  location  with  respect  to  the  base  frame  is  identified  by  the 
homogeneous  transformation  matrix  Tbc.  Take  a  point  of  the  object  of  coor¬ 
dinates  pc  =  [p%  Py  p%  ]  ;  typically,  the  centroid  of  the  object  is  chosen. 
Then,  the  coordinate  transformation  from  the  base  frame  to  the  camera  frame 
is  described  as 

P  =  Tcbp ,  (5.35) 

where  p  denotes  the  object  position  with  respect  to  the  base  frame  and  ho¬ 
mogeneous  representations  of  vectors  have  been  used. 
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A  reference  frame  can  be  introduced  on  the  image  plane,  whose  axes  X 
and  Y  are  parallel  to  the  axes  xc  and  yc  of  the  camera  frame,  and  the  origin  is 
at  the  intersection  of  the  optical  axis  with  the  image  plane,  termed  principal 
point.  Due  to  the  refraction  phenomenon,  the  point  in  the  camera  frame  is 
transformed  into  a  point  in  the  image  plane  via  the  perspective  transformation, 
i.e., 


Xf  =  ~ 
Yf  =  ~ 


fPx 

Pz 

M. 

Pz 


where  ( Xf,Yf )  are  the  new  coordinates  in  the  frame  defined  on  the  image 
plane,  and  /  is  the  focal  length  of  the  lens.  Notice  that  these  coordinates  are 
expressed  in  metric  units  and  the  above  transformation  is  singular  at  pcz  =  0. 

The  presence  of  the  minus  sign  in  the  equations  of  the  perspective  transfor¬ 
mation  is  consistent  with  the  fact  that  the  image  of  an  object  appears  upside 
down  on  the  image  plane  of  the  camera.  Such  an  effect  can  be  avoided,  for 
computational  ease,  by  considering  a  virtual  image  plane  positioned  before 
the  lens,  in  correspondence  of  the  plane  zc  —  foi  the  camera  frame.  In  this 
way,  the  model  represented  in  Fig.  5.24  is  obtained,  which  is  characterized  by 
the  frontal  perspective  transformation 
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where,  with  abuse  of  notation,  the  name  of  the  variables  on  the  virtual  plane 
has  not  been  changed. 

These  relationships  hold  only  in  theory,  since  the  real  lenses  are  always 
affected  by  imperfections,  which  cause  image  quality  degradation.  Two  types 
of  distortions  can  be  recognized,  namely,  aberrations  and  geometric  distortion. 
The  former  can  be  reduced  by  restricting  the  light  rays  to  a  small  central 
region  of  the  lens;  the  effects  of  the  latter  can  be  compensated  on  the  basis  of 
a  suitable  model  whose  parameters  are  to  be  identified. 

A  visual  information  is  typically  elaborated  by  a  digital  processor,  and 
thus  the  measurement  principle  is  to  transform  the  light  intensity  I(X,Y)  of 
each  point  in  the  image  plane  into  a  number.  It  is  clear  that  a  spatial  sampling 
is  needed  since  an  infinite  number  of  points  in  the  image  plane  exist,  as  well 
as  a  temporal  sampling  since  the  image  can  change  during  time.  The  CCD  or 
CMOS  sensors  play  the  role  of  spatial  samplers,  while  the  shutter  in  front  of 
the  lens  plays  the  role  of  the  temporal  sampler. 

The  spatial  sampling  unit  is  the  pixel,  and  thus  the  coordinates  ( X ,  Y)  of  a 
point  in  the  image  plane  are  to  be  expressed  in  pixels,  i.e.,  (A/,  Yj).  Due  to  the 


228 


5  Actuators  and  Sensors 


Fig.  5.24.  Frontal  perspective  transformation 


photosite  finite  dimensions,  the  pixel  coordinates  of  the  point  are  related  to 
the  coordinates  in  metric  units  through  two  scale  factors  ax  and  ay,  namely, 

Xj  =  - +  X0  (5.38) 

Pz 

Yi  =  +  Yo,  (5.39) 

Pz 

where  Xq  and  Yo  are  the  offsets  which  take  into  account  the  position  of  the 
origin  of  the  pixel  coordinate  system  with  respect  to  the  optical  axis.  This 
nonlinear  transformation  can  be  written  in  a  linear  form  by  resorting  to  the 
homogeneous  representation  of  the  point  (xj,yj,zj)  via  the  relationships 
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where  A  >  0.  As  a  consequence,  (5.38),  (5.39)  can  be  rewritten  as 
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At  this  point,  the  overall  transformation  from  the  Cartesian  space  of  the 
observed  object  to  the  image  space  of  its  image  in  pixels  is  characterized  by 
composing  the  transformations  in  (5.35),  (5.40)  as 

s'  =  finrcb  (5.43) 

which  represents  the  so-called  camera  calibration  matrix.  It  is  worth  pointing 
out  that  such  a  matrix  contains  intrinsic  parameters  (ax,  ay,  Xq,  Yq,  f)  in  fl 
depending  on  the  sensor  and  lens  characteristics  as  well  as  extrinsic  parame¬ 
ters  in  Tbc  depending  on  the  relative  position  and  orientation  of  the  camera 
with  respect  to  the  base  frame.  Several  calibration  techniques  exist  to  iden¬ 
tify  these  parameters  in  order  to  compute  the  transformation  between  the 
Cartesian  space  and  the  image  space  as  accurately  as  possible. 

If  the  intrinsic  parameters  of  a  camera  are  known,  from  a  computation¬ 
ally  viewpoint,  it  is  convenient  to  refer  to  the  normalized  coordinates  (X,  Y), 
defined  by  the  normalized  perspective  transformation 
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These  coordinates  are  defined  in  metrical  units  and  coincide  with  the  coor¬ 
dinates  (5.36),  (5.37)  in  the  case  when  /  =  1.  Comparing  (5.40)  with  (5.44) 
yields  the  invertible  transformation 
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relating  the  normalized  coordinates  to  those  expressed  in  pixels  through  the 
matrix  of  intrinsic  parameters. 

If  a  monochrome  CCD  camera4  is  of  concern,  the  output  amplifier  of  the 
sensor  produces  a  signal  which  is  processed  by  a  timing  analog  electronics 
in  order  to  generate  an  electric  signal  according  to  one  of  the  existing  video 
standards,  i.e. ,  the  CCIR  European  and  Australian  standard,  or  the  RS170 
American  and  Japanese  standard.  In  any  case,  the  video  signal  is  a  voltage  of 
1 V  peak-to-peak  whose  amplitude  represents  sequentially  the  image  intensity. 

The  entire  image  is  divided  into  a  number  of  lines  (625  for  the  CCIR 
standard  and  525  for  the  RS170  standard)  to  be  sequentially  scanned.  The 
raster  scan  proceeds  horizontally  across  each  line  and  each  line  from  top  to 
bottom,  but  first  all  the  even  lines,  forming  the  first  field ,  and  then  all  the  odd 
lines,  forming  the  second  field ,  so  that  a  frame  is  composed  of  two  successive 

4  Colour  cameras  are  equipped  with  special  CCDs  sensitive  to  three  basic  colours 
(RGB);  the  most  sophisticated  cameras  have  three  separate  sensors,  one  per  each 
basic  colour. 
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fields.  This  technique,  called  interlacing ,  allows  the  image  to  be  updated  either 
at  frame  rate  or  at  field  rate;  in  the  former  case  the  update  frequency  is  that 
of  the  entire  frame  (25  Hz  for  the  CCIR  standard  and  30  Hz  for  the  RS170 
standard),  while  in  the  latter  case  the  update  frequency  can  be  doubled  as 
long  as  half  the  vertical  resolution  can  be  tolerated. 

The  last  step  of  the  measurement  process  is  to  digitize  the  analog  video 
signal.  The  special  analog-to-digital  converters  adopted  for  video  signal  acqui¬ 
sition  are  called  frame  grabbers.  By  connecting  the  output  of  the  camera  to  the 
frame  grabber,  the  video  waveform  is  sampled  and  quantized  and  the  values 
stored  in  a  two-dimensional  memory  array  representing  the  spatial  sample  of 
the  image,  known  as  framestore ;  this  array  is  then  updated  at  field  or  frame 
rate. 

In  the  case  of  CMOS  cameras  (currently  available  only  for  monochrome 
images),  thanks  to  CMOS  technology  which  allows  the  integration  of  the 
analog-to-digital  converter  in  each  pixel,  the  output  of  the  camera  is  directly 
a  two-dimensional  array,  whose  elements  can  be  accessed  randomly.  Such  ad¬ 
vantage,  with  respect  to  CCD  cameras,  leads  to  the  possibility  of  higher  frame 
rates  if  only  parts  of  the  entire  frame  are  accessed. 

The  sequence  of  steps  from  image  formation  to  image  acquisition  described 
above  can  be  classified  as  a  process  of  low-level  vision ;  this  includes  the  extrac¬ 
tion  of  elementary  image  features,  e.g.,  centroid  and  intensity  discontinuities. 
On  the  other  hand,  a  robotic  system  can  be  considered  really  autonomous 
only  if  procedures  for  emulating  cognition  are  available,  e.g.,  recognizing  an 
observed  object  among  a  set  of  CAD  models  stored  into  a  data  base.  In  this 
case,  the  artificial  vision  process  can  be  referred  to  as  high-level  vision. 
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Problems 

5.1.  Prove  (5.7)-(5.10). 

5.2.  Consider  the  DC  servomotor  with  the  data:  Im  =  0.0014  kg-m2,  Fm  = 
0.01  N-m-s/rad,  La  =  2mH,  Ra  =  0.2  ohm,  fct  =  0.2N-m/A,  kv  =  0.2  V-s/rad, 
CiGv  =  1,  Tv  =  0.1ms,  ki  =  0.  Perform  a  computer  simulation  of  the  current 
and  velocity  response  to  a  unit  step  voltage  input  V'c.  Adopt  a  sampling  time 
of  1  ms. 


5.3.  For  the  servomotor  of  the  previous  problem,  design  the  controller  of  the 
current  loop  Ci(s)  so  that  the  current  response  to  a  unit  step  voltage  input 
V'c  is  characterized  by  a  settling  time  of  2  ms.  Compare  the  velocity  response 
with  that  obtained  in  Problem  5.2. 


5.4.  Find  the  control  voltage/output  position  and  reaction  torque/output  po¬ 
sition  transfer  functions  for  the  scheme  of  Fig.  5.6. 

5.5.  For  a  Gray-code  optical  encoder,  find  the  interconversion  logic  circuit 
which  yields  a  binary-coded  output  word. 


5.6.  With  reference  to  a  contact  situation  of  the  kind  illustrated  in  Fig. 
let 


rcs  =  [ — 0.3  0  0.2  ]Tm  R!; 


0  0  1 
0-10 
10  0 


5.16, 


and  let  the  force  sensor  measurement  be 


fs  =  [ 20  0  0]tN  /x®  =  [0  6  0]T  N-m. 


Compute  the  equivalent  force  and  moment  in  the  contact  frame. 

5.7.  Consider  the  SCARA  manipulator  in  Fig.  2.34  with  link  lengths  a\  = 
a 2  =  0.5  m.  Let  the  base  frame  be  located  at  the  intersection  between  the 
first  link  and  the  base  link  with  axis  2  pointing  downward  and  axis  x  in 
the  direction  of  the  first  link  when  =  0.  Assume  that  a  CCD  camera 
is  mounted  on  the  wrist  so  that  the  camera  frame  is  aligned  with  the  end- 
effector  frame.  The  camera  parameters  are  /  =  8  mm,  ax  =  79.2  pixel/mm, 
ay  =  120.5  pixel/mm,  X0  =  250,  Y0  =  250.  An  object  is  observed  by  the 
camera  and  is  described  by  the  point  of  coordinates  p  =  [0.8  0.5  0.9  ]Tm. 

Compute  the  pixel  coordinates  of  the  point  when  the  manipulator  is  at  the 
configuration  q  =  [  0  7t/4  0.1  0]T. 
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Control  Architecture 


This  chapter  is  devoted  to  presenting  a  reference  model  for  the  functional 
architecture  of  an  industrial  robot’s  control  system.  The  hierarchical  structure 
and  its  articulation  into  functional  modules  allows  the  determination  of  the 
requirements  and  characteristics  of  the  programming  environment  and  the 
hardware  architecture.  The  architecture  refers  to  robot  manipulators,  yet  its 
articulation  in  levels  also  holds  for  mobile  robots. 


6.1  Functional  Architecture 

The  control  system  to  supervise  the  activities  of  a  robotic  system  should  be 
endowed  with  a  number  of  tools  providing  the  following  functions: 

•  capability  of  moving  physical  objects  in  the  working  environment,  i.e. , 
manipulation  ability; 

•  capability  of  obtaining  information  on  the  state  of  the  system  and  working 
environment,  i.e.,  sensory  ability; 

•  capability  of  exploiting  information  to  modify  system  behaviour  in  a  pre¬ 
programmed  manner,  i.e.,  intelligence  ability; 

•  capability  of  storing,  elaborating  and  providing  data  on  system  activity, 
i.e.,  data  processing  ability. 

An  effective  implementation  of  these  functions  can  be  obtained  by  means 
of  a  functional  architecture  which  is  thought  of  as  the  superposition  of  several 
activity  levels  arranged  in  a  hierarchical  structure.  The  lower  levels  of  the 
structure  are  oriented  to  physical  motion  execution,  whereas  the  higher  levels 
are  oriented  to  logical  action  planning.  The  levels  are  connected  by  data  flows; 
those  directed  towards  the  higher  levels  regard  measurements  and/or  results 
of  actions,  while  those  directed  towards  the  lower  levels  regard  transmission 
of  directions. 

With  reference  to  the  control  system  functions  implementing  management 
of  the  above  listed  system  activities,  in  general  it  is  worth  allocating  three 
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functional  models  at  each  level.  A  first  module  is  devoted  to  sensory  data 
management  (sensory  module) .  A  second  module  is  devoted  to  provide  knowl¬ 
edge  of  the  relevant  world  (modelling  module).  A  third  module  is  devoted  to 
decide  the  policy  of  the  action  (decision  module). 

More  specifically,  the  sensory  modules  acquire,  elaborate,  correlate  and 
integrate  sensory  data  in  time  and  space,  in  order  to  recognize  and  measure 
the  system  state  and  environment  characteristic;  clearly,  the  functions  of  each 
module  are  oriented  to  the  management  of  the  relevant  sensory  data  for  that 
level. 

On  the  other  hand,  the  modelling  modules  contain  models  derived  on  the 
basis  of  a  priori  knowledge  of  system  and  environment;  these  models  are  up¬ 
dated  by  the  information  coming  from  the  sensory  modules,  while  the  activa¬ 
tion  of  the  required  functions  is  entrusted  to  the  decision  modules. 

Finally,  the  decision  modules  perform  breakdown  of  high-level  tasks  into 
low-level  actions;  such  task  breakdown  concerns  both  breakdown  in  time  of 
sequential  actions  and  breakdown  in  space  of  concurrent  actions.  Each  decision 
module  is  entrusted  with  the  functions  concerning  management  of  elementary 
action  assignments,  task  planning  and  execution. 

The  functions  of  a  decision  module  characterize  the  level  of  the  hierarchy 
and  determine  the  functions  required  to  the  modelling  and  sensory  modules 
operating  at  the  same  level.  This  implies  that  the  contents  of  these  two  mod¬ 
ules  do  not  uniquely  allow  the  determination  of  the  hierarchical  level,  since 
the  same  function  may  be  present  at  more  levels  depending  on  the  needs  of 
the  decision  modules  at  the  relative  levels. 

The  functional  architecture  needs  an  operator  interface  at  each  level  of  the 
hierarchy,  so  as  to  allow  an  operator  to  perform  supervisory  and  intervention 
functions  on  the  robotic  system. 

The  instructions  imparted  to  the  decision  module  at  a  certain  level  may 
be  provided  either  by  the  decision  module  at  the  next  higher  level  or  by 
the  operator  interface,  or  else  by  a  combination  of  the  two.  Moreover,  the 
operator,  by  means  of  suitable  communication  tools,  can  be  informed  on  the 
system  state  and  thus  can  contribute  his/her  own  knowledge  and  decisions  to 
the  modelling  and  sensory  modules. 

In  view  of  the  high  data  flow  concerning  the  exchange  of  information  be¬ 
tween  the  various  levels  and  modules  of  the  functional  architecture,  it  is  worth 
allocating  a  shared  global  memory  which  contains  the  updated  estimates  on 
the  state  of  the  whole  system  and  environment. 

The  structure  of  the  reference  model  for  the  functional  architecture  is 
represented  in  Fig.  6.1,  where  the  four  hierarchical  levels  potentially  relevant 
for  robotic  systems  in  industrial  applications  are  illustrated.  Such  levels  regard 
definition  of  the  task ,  its  breakdown  into  elementary  actions,  assignment  of 
primitives  to  the  actions,  and  implementation  of  control  actions  on  the  servo- 
manipulator.  In  the  following,  the  general  functions  of  the  three  modules  at 
each  level  are  described. 
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Fig.  6.1.  Reference  model  for  a  control  system  functional  architecture 


At  the  task  level,  the  user  specifies  the  task  which  the  robotic  system 
should  execute;  this  specification  is  performed  at  a  high  level  of  abstraction. 
The  goal  of  the  desired  task  is  analyzed  and  broken  down  into  a  sequence  of 
actions  which  are  coordinated  in  space  and  time  and  allow  implementation  of 
the  task.  The  choice  of  actions  is  performed  on  the  basis  of  knowledge  models 
as  well  as  of  the  scene  of  interest  for  the  task.  For  instance,  consider  the 
application  of  a  robot  installed  in  an  assembly  line  which  is  required  to  perform 
a  specific  assembly  task.  To  define  the  elementary  actions  that  have  to  be 
transmitted  to  the  decision  module  at  the  next  lower  level,  the  decision  module 
should  consult  its  knowledge  base  available  in  the  modelling  module,  e.g.,  type 
of  assembly,  components  of  the  object  to  assembly,  assembly  sequence,  and 
choice  of  tools.  This  knowledge  base  should  be  continuously  updated  by  the 
information  provided  by  the  sensory  module  concerning  location  of  the  parts 
to  assembly;  such  information  is  available  by  means  of  a  high-level  vision 
system  operating  in  a  scarcely  structured  environment,  or  else  by  means  of 
simple  sensors  detecting  the  presence  of  an  object  in  a  structured  environment. 

At  the  action  level,  the  symbolic  commands  coming  from  the  task  level 
are  translated  into  sequences  of  intermediate  configurations  which  character¬ 
ize  a  motion  path  for  each  elementary  action.  The  choice  of  the  sequences  is 
performed  on  the  basis  of  models  of  the  manipulator  and  environment  where 
the  action  is  to  take  place.  With  reference  to  one  of  the  actions  generated  by 
the  above  assembly  task,  the  decision  module  chooses  the  most  appropriate 
coordinate  system  to  compute  manipulator’s  end-effector  poses,  by  separating 
translation  from  rotation  if  needed;  it  decides  whether  to  operate  in  the  joint 
or  operational  space,  it  computes  the  path  or  via  points,  and  for  the  latter  it 
defines  the  interpolation  functions.  By  doing  so,  the  decision  module  should 
compare  the  sequence  of  configurations  with  a  model  of  the  manipulator  as 
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well  as  with  a  geometric  description  of  the  environment,  which  are  both  avail¬ 
able  in  the  modelling  model.  In  this  way,  action  feasibility  is  ascertained  in 
terms  of  obstacle-collision  avoidance,  motion  in  the  neighbourhood  of  kine¬ 
matically  singular  configurations,  occurrence  of  mechanical  joint  limits,  and 
eventually  utilization  of  available  redundant  DOFs.  The  knowledge  base  is  up¬ 
dated  by  the  information  on  the  portion  of  scene  where  the  single  action  takes 
place  which  is  provided  by  the  sensory  module,  e.g.,  by  means  of  a  low-level 
vision  system  or  range  sensors. 

At  the  primitive  level,  on  the  basis  of  the  sequence  of  configurations  re¬ 
ceived  by  the  action  level,  admissible  motion  trajectories  are  computed  and 
the  control  strategy  is  decided.  The  motion  trajectory  is  interpolated  so  as  to 
generate  the  references  for  the  servo  level.  The  choice  of  motion  and  control 
primitives  is  conditioned  by  the  features  of  the  mechanical  structure  and  its 
degree  of  interaction  with  the  environment.  Still  with  reference  to  the  above 
case  study,  the  decision  module  computes  the  geometric  path  and  the  rel¬ 
ative  trajectory  on  the  basis  of  the  knowledge  of  the  manipulator  dynamic 
model  available  in  the  modelling  module.  Moreover,  it  defines  the  type  of  con¬ 
trol  algorithm,  e.g.,  decentralized  control,  centralized  control,  or  interaction 
control;  it  specifies  the  relative  gains;  and  it  performs  proper  coordinate  trans¬ 
formations,  e.g.,  kinematic  inversion  if  needed.  The  sensory  module  provides 
information  on  the  occurrence  of  conflicts  between  motion  planning  and  exe¬ 
cution,  by  means  of,  e.g.,  force  sensors,  low-level  vision  systems  and  proximity 
sensors. 

At  the  servo  level,  on  the  basis  of  the  motion  trajectories  and  control 
strategies  imparted  by  the  primitive  level,  control  algorithms  are  implemented 
which  provide  the  driving  signals  to  the  joint  servomotors.  The  control  algo¬ 
rithm  operates  on  error  signals  between  the  reference  and  the  actual  values 
of  the  controlled  quantities,  by  utilizing  knowledge  of  manipulator  dynamic 
model,  and  of  kinematics  if  needed.  In  particular,  the  decision  module  per¬ 
forms  a  microinterpolation  on  the  reference  trajectory  to  exploit  fully  the 
dynamic  characteristic  of  the  drives;  it  computes  the  control  law,  and  it  gen¬ 
erates  the  (voltage  or  current)  signals  for  controlling  the  specific  drives.  The 
modelling  module  elaborates  the  terms  of  the  control  law  depending  on  the 
manipulator  current  configuration  and  pass  them  to  the  decision  module;  such 
terms  are  computed  on  the  basis  of  knowledge  of  manipulator  dynamic  model. 
Finally,  the  sensory  module  provides  measurements  of  the  proprioceptive  sen¬ 
sors  (position,  velocity  and  contact  force  if  needed);  these  measurements  are 
used  by  the  decision  module  to  compute  the  servo  errors  and,  if  required, 
by  the  modelling  module  to  update  the  configuration-dependent  terms  in  the 
model. 

The  specification  of  the  functions  associated  with  each  level  points  out  that 
the  implementation  of  such  functions  should  be  performed  at  different  time 
rates,  in  view  of  their  complexity  and  requirements.  On  one  hand,  the  func¬ 
tions  associated  with  the  higher  levels  are  not  subject  to  demanding  real-time 
constraints,  since  they  regard  planning  activities.  On  the  other  hand,  their 
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Fig.  6.2.  Hierarchical  levels  of  a  functional  architecture  for  industrial  robots 


complexity  is  notable,  since  scheduling,  optimization,  resource  management 
and  high-level  sensory  system  data  processing  are  required  to  update  complex 
models. 

At  the  lowest  level,  demanding  real-time  operation  prevails  in  order  to 
obtain  high  dynamic  performance  of  the  mechanical  structure.  The  above 
remarks  lead  to  the  conclusion  that,  at  the  servo  level,  it  is  necessary  to 
provide  the  driving  commands  to  the  motors  and  to  detect  the  proprioceptive 
sensory  data  at  sampling  rates  of  the  order  of  the  millisecond,  while  sampling 
rates  of  the  order  of  the  minute  are  admissible  at  the  task  level. 

With  respect  to  this  reference  model  of  functional  architecture,  current 
industrial  robot’s  control  systems  are  not  endowed  with  all  the  functions  il¬ 
lustrated,  because  of  both  technology  and  cost  limitations.  In  this  regard, 
the  task  level  is  not  implemented  at  all  since  there  do  not  yet  exist  effective 
and  reliable  application  software  packages  allowing  support  of  the  complex 
functions  required  at  this  level. 

It  is  worth  characterizing  those  functional  levels  of  the  reference  models 
which  are  typically  implemented  in  advanced  industrial  robot's  control  sys¬ 
tems.  The  details  of  Fig.  6.2  show  what  follows: 
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•  The  modelling  and  sensory  modules  are  always  present  at  the  lowest  level, 
because  of  demanding  requirements  at  the  servo  level  for  high  dynamic 
performance  robots  to  be  employed  even  in  relatively  simple  applications. 

•  At  the  primitive  level,  the  modelling  module  is  usually  present  while  the 
sensory  module  is  present  only  in  a  reduced  number  of  applications  that 
require  robot  interaction  with  a  less  structured  environment. 

•  At  the  action  level,  the  decision  module  is  present  only  as  an  interpreter  of 
the  high-level  commands  imparted  by  the  operator.  All  the  task  breakdown 
functions  are  entrusted  to  the  operator,  and  thus  the  modelling  and  sensory 
module  are  absent  at  this  level.  Possible  checking  of  action  feasibility  is 
moved  down  to  the  primitive  level  where  a  modelling  module  exists. 

In  view  of  the  highly-structured  reference  model  of  functional  architecture 
illustrated  above,  evolution  of  the  control  system  towards  more  and  more  pow¬ 
erful  capabilities  is  possible.  In  fact,  one  may  foresee  that  information  technol¬ 
ogy  progress  may  allow  the  addition  of  hierarchically  higher  levels  than  the 
task  level.  These  should  functionally  characterize  complex  tasks  to  be  bro¬ 
ken  down  into  elementary  tasks  and  yet,  at  an  even  higher  level,  missions  to 
be  broken  down  into  complex  tasks.  A  six-level  hierarchical  structure  of  the 
above  kind  has  been  proposed  as  the  reference  model  for  the  functional  archi¬ 
tecture  of  the  control  system  of  a  service  robotic  system  for  space  applications 
(NASREM).  In  this  framework,  one  may  allocate  the  functions  required  to  ad¬ 
vanced  robotics  systems  devoted  to  field  or  service  applications,  as  discussed 
in  Sect.  1.4. 


6.2  Programming  Environment 

Programming  a  robotic  system  requires  definition  of  a  programming  environ¬ 
ment  supported  by  suitable  languages ,  which  allows  the  operator  imparting 
the  task  directions  that  the  robot  should  execute.  The  programming  envi¬ 
ronment  is  entrusted  not  only  with  the  function  of  translating  statements  by 
means  of  a  suitable  language,  but  also  with  the  function  of  checking  correct 
execution  of  a  task  being  executed  by  the  robot.  Therefore,  robot  program¬ 
ming  environments,  besides  having  some  features  in  common  with  computer 
programming  environments,  present  a  number  of  issues  related  to  the  observa¬ 
tion  that  program  execution  produces  effects  on  the  physical  world.  In  other 
words,  even  if  a  very  accurate  description  of  physical  reality  is  available  in 
the  programming  environment,  a  number  of  situations  will  unavoidably  occur 
which  have  not  been  or  cannot  be  predicted. 

As  a  consequence,  a  robot  programming  environment  should  be  endowed 
with  the  following  features: 

•  real-time  operating  system, 

•  world  modelling, 

•  motion  control, 
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•  sensory  data  reading, 

•  interaction  with  physical  system, 

•  error  detection  capability, 

•  recovery  of  correct  operational  functions, 

•  specific  language  structure. 

Therefore,  the  requirements  on  a  programming  environment  may  natu¬ 
rally  stem  from  the  articulation  into  models  of  the  preceding  reference  model 
of  functional  architecture.  Such  an  environment  will  be  clearly  conditioned 
by  the  level  of  the  architecture  at  which  operator  access  is  allowed.  In  the 
following,  the  requirements  imposed  on  the  programming  environment  by  the 
functions  respectively  characterizing  the  sensory,  modelling  and  decision  mod¬ 
ules  are  presented  with  reference  to  the  hierarchical  levels  of  the  functional 
architecture. 

Sensory  data  handling  is  the  determining  factor  which  qualifies  a  program¬ 
ming  environment.  At  the  servo  level,  real-time  proprioceptive  sensory  data 
conditioning  is  required.  At  the  primitive  level,  sensory  data  have  to  be  ex¬ 
pressed  in  the  relevant  reference  frames.  At  the  action  level,  geometric  features 
of  the  objects  interested  to  the  action  have  to  be  extracted  by  high-level  sen¬ 
sory  data.  At  the  task  level,  tools  allowing  recognition  of  the  objects  present 
in  the  scene  are  required. 

The  ability  of  consulting  knowledge  models  is  a  support  for  a  programming 
environment.  At  the  servo  level,  on-line  numerical  computation  of  the  models 
utilized  by  control  algorithms  is  to  be  performed  on  the  basis  of  sensory  data. 
At  the  primitive  level,  coordinate  transformations  have  to  be  operated.  At 
the  action  level,  it  is  crucial  to  have  tools  allowing  system  simulation  and 
CAD  modelling  of  elementary  objects.  At  the  task  level,  the  programming 
environment  should  assume  the  functions  of  an  expert  system. 

Decision  functions  play  a  fundamental  role  in  a  programming  environ¬ 
ment,  since  they  allow  the  definition  of  the  flow  charts.  At  the  servo  level, 
on-line  computation  ability  is  required  to  generate  the  driving  signals  for  the 
mechanical  system.  At  the  primitive  level,  logic  conditioning  is  to  be  present. 
At  the  action  level,  process  synchronization  options  should  be  available  in 
order  to  implement  nested  loops,  parallel  computation  and  interrupt  system. 
At  the  task  level,  the  programming  environment  should  allow  management  of 
concurrent  processes,  and  it  should  be  endowed  with  tools  to  test  for,  locate 
and  remove  mistakes  from  a  program  (debuggers)  at  a  high-interactive  level. 

The  evolution  of  programming  environments  has  been  conditioned  by 
technology  development  of  computer  science.  An  analysis  of  this  evolution 
leads  to  finding  three  generations  of  environments  with  respect  to  their  func¬ 
tional  characteristics,  namely,  teaching-by-showing ,  robot -oriented  program¬ 
ming ,  and  object-oriented  programming .  In  the  evolution  of  the  environments, 
the  next  generation  usually  incorporates  the  functional  characteristics  of  the 
previous  generation. 
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This  classification  regards  those  features  of  the  programming  environment 
relative  to  the  operator  interface,  and  thus  it  has  a  direct  correspondence 
with  the  hierarchical  levels  of  the  reference  model  of  functional  architecture. 
The  functions  associated  with  the  servo  level  lead  to  understanding  that  a 
programming  environment  problem  does  not  really  exist  for  the  operator. 
In  fact,  low-level  programming  concerns  the  use  of  traditional  programming 
languages  (Assembly,  C)  for  development  of  real-time  systems.  The  operator 
is  only  left  with  the  possibility  of  intervening  by  means  of  simple  command 
actuation  (point-to-point,  reset),  reading  of  proprioceptive  sensory  data,  and 
limited  editing  capability. 

6.2.1  Teaching-by-Showing 

The  first  generation  has  been  characterized  by  programming  techniques  of 
teaching-by- showing  type.  The  operator  guides  the  manipulator  manually  or 
by  means  of  a  teach  pendant  along  the  desired  motion  path.  During  motion 
execution,  the  data  read  by  joint  position  transducers  are  stored  and  thus 
they  can  be  utilized  later  as  references  for  the  joint  drive  servos;  in  this  way, 
the  mechanical  structure  is  capable  of  executing  (playing  back)  the  motion 
taught  by  a  direct  acquisition  on  the  spot. 

The  programming  environment  does  not  allow  implementation  of  logic 
conditioning  and  queuing,  and  thus  the  associated  computational  hardware 
plays  elementary  functions.  The  operator  is  not  required  to  have  special  pro¬ 
gramming  skill,  and  thus  he/she  can  be  a  plant  technician.  The  set-up  of  a 
working  program  obviously  requires  the  robot  to  be  available  to  the  operator 
at  the  time  of  teaching,  and  thus  the  robot  itself  has  to  be  taken  off  produc¬ 
tion.  Typical  applications  that  can  be  solved  by  this  programming  technique 
include  spot  welding,  spray  painting  and,  in  general,  simple  palletizing. 

With  regard  to  the  reference  model  of  functional  architecture,  a  program¬ 
ming  environment  based  on  the  teaching-by-showing  technique  allows  opera¬ 
tor  access  at  the  primitive  level. 

The  drawbacks  of  such  an  environment  may  be  partially  overcome  by  the 
adoption  of  simple  programming  languages  which  allow: 

•  the  acquisition  of  a  meaningful  posture  by  teaching, 

•  the  computation  of  the  end-effector  pose  with  respect  to  a  reference  frame, 
by  means  of  a  direct  kinematics  transformation, 

•  the  assignment  of  a  motion  primitive  and  the  trajectory  parameters  (usu¬ 
ally,  velocity  as  a  percentage  of  the  maximum  velocity), 

•  the  computation  of  the  servo  references,  by  means  of  an  inverse  kinematics 
transformation, 

•  the  teaching  sequences  to  be  conditioned  to  the  use  of  simple  external 
sensors  (presence  of  an  object  at  the  gripper), 

•  the  correction  of  motion  sequences  by  using  simple  text  editors, 

•  simple  connections  to  be  made  between  subsets  of  elementary  sequences. 
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Providing  a  teaching-by-showing  environment  with  the  the  above-listed 
functions  can  be  framed  as  an  attempt  to  develop  a  structured  programming 
environment. 

6.2.2  Robot-oriented  Programming 

Following  the  advent  of  efficient  low-cost  computational  means,  robot-oriented 
programming  environments  have  been  developed.  The  need  for  interaction 
of  the  environment  with  physical  reality  has  imposed  integration  of  several 
functions,  typical  of  high-level  programming  languages  (BASIC,  PASCAL), 
with  those  specifically  required  by  robotic  applications.  In  fact,  many  robot- 
oriented  languages  have  retained  the  teaching-by-showing  programming  mode, 
in  view  of  its  natural  characteristic  of  accurate  interface  with  the  physical 
world. 

Since  the  general  framework  is  that  of  a  computer  programming  environ¬ 
ment,  two  alternatives  have  been  considered: 

•  to  develop  ad  hoc  languages  for  robotic  applications, 

•  to  develop  robot  program  libraries  supporting  standard  programming  lan¬ 
guages. 

The  current  situation  features  the  existence  of  numerous  new  proprietary 
languages,  whereas  it  would  be  desirable  to  develop  either  robotic  libraries 
to  be  used  in  the  context  of  consolidated  standards,  or  new  general-purpose 
languages  for  industrial  automation  applications. 

Robot-oriented  languages  are  structured  programming  languages  which  in¬ 
corporate  high-level  statements  and  have  the  characteristic  of  an  interpreted 
language,  in  order  to  obtain  an  interactive  environment  allowing  the  pro¬ 
grammer  to  check  the  execution  of  each  source  program  statement  before 
proceeding  to  the  next  one.  Common  features  of  such  languages  are: 

•  text  editor, 

•  complex  data  representation  structures, 

•  extensive  use  of  predefined  state  variable, 

•  execution  of  matrix  algebra  operations, 

•  extensive  use  of  symbolic  representations  for  coordinate  frames, 

•  possibility  to  specify  the  coordinated  motion  of  more  frames  rigidly  at¬ 
tached  to  objects  by  means  of  a  single  frame, 

•  inclusion  of  subroutines  with  data  and  parameter  exchange, 

•  use  of  logic  conditioning  and  queuing  by  means  of  flags, 

•  capability  of  parallel  computing, 

•  functions  of  programmable  logic  controller  (PLC). 

With  respect  to  the  reference  model  of  functional  architecture,  it  can  be 
recognized  that  a  robot-oriented  programming  environment  allows  operator 
access  at  the  action  level. 
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In  view  of  the  structured  language  characteristic,  the  operator  in  this  case 
should  be  an  expert  language  programmer.  Editing  an  application  program 
may  be  performed  off  line,  i.e.,  without  physical  availability  of  the  robot  to  the 
operator;  off-line  programming  demands  a  perfectly  structured  environment, 
though.  A  robotic  system  endowed  with  a  robot-oriented  programming  lan¬ 
guage  allows  execution  of  complex  applications  where  the  robot  is  inserted  in 
a  work  cell  and  interacts  with  other  machines  and  devices  to  perform  complex 
tasks,  such  as  part  assembly. 

Finally,  a  programming  environment  that  allows  access  at  the  task  level 
of  a  reference  model  of  functional  architecture  is  characterized  by  an  object- 
oriented  language.  Such  an  environment  should  have  the  capability  of  specify¬ 
ing  a  task  by  means  of  high-level  statements  allowing  automatic  execution  of 
a  number  of  actions  on  the  objects  present  in  the  scene.  Robot  programming 
languages  belonging  to  this  generation  are  currently  under  development  and 
thus  they  are  not  yet  available  on  the  market.  They  can  be  framed  in  the  field 
of  expert  systems  and  artificial  intelligence. 


6.3  Hardware  Architecture 

The  hierarchical  structure  of  the  functional  architecture  adopted  as  a  refer¬ 
ence  model  for  an  industrial  robot’s  control  system,  together  with  its  artic¬ 
ulation  into  different  functional  modules,  suggests  hardware  implementation 
which  exploits  distributed  computational  resources  interconnected  by  means 
of  suitable  communication  channels.  To  this  end,  it  is  worth  recalling  that  the 
functions  implemented  in  current  control  systems  regard  the  three  levels  from 
servo  to  action,  with  a  typically  limited  development  of  the  functions  imple¬ 
mented  at  the  action  level.  At  the  servo  and  primitive  levels,  computational 
capabilities  are  required  with  demanding  real-time  constraints. 

A  general  model  of  the  hardware  architecture  for  the  control  system  of 
an  industrial  robot  is  illustrated  in  Fig.  6.3.  In  this  figure,  proper  boards 
with  autonomous  computational  capabilities  have  been  associated  with  the 
functions  indicated  in  the  reference  model  of  functional  architecture  of  Fig.  9.2. 
The  boards  are  connected  to  a  bus,  e.g.,  a  VME  bus,  which  allows  support  of 
the  communication  data  flow;  the  bus  bandwidth  should  be  wide  enough  so 
as  to  satisfy  the  requirements  imposed  by  real-time  constraints. 

The  system  board  is  typically  a  CPU  endowed  with: 

•  a  microprocessor  with  mathematical  coprocessor, 

•  a  bootstrap  EPROM  memory, 

•  a  local  RAM  memory, 

•  a  RAM  memory  shared  with  the  other  boards  through  the  bus, 

•  a  number  of  serial  and  parallel  ports  interfacing  the  bus  and  the  external 
world, 

•  counters,  registers  and  timers, 
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Fig.  6.3.  General  model  of  the  hardware  architecture  of  an  industrial  robot’s  control 
system 


•  an  interrupt  system. 

The  following  functions  are  to  be  implemented  in  the  system  board: 

•  operator  interface  through  teach  pendant,  keyboard,  video  and  printer, 

•  interface  with  an  external  memory  (hard  disk)  used  to  store  data  and 
application  programs, 

•  interface  with  workstations  and  other  control  systems  by  means  of  a  local 
communication  network,  e.g.,  Ethernet, 

•  I/O  interface  with  peripheral  devices  in  the  working  area,  e.g.,  feeders, 
conveyors  and  ON/OFF  sensors, 

•  system  bootstrap, 

•  programming  language  interpreter, 

•  bus  arbiter. 

The  other  boards  facing  the  bus  may  be  endowed,  besides  the  basic  com¬ 
ponents  of  the  system  board,  with  a  supplementary  or  alternative  processor 
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(DSP,  Transputer)  for  implementation  of  computationally  demanding  or  ded¬ 
icated  functions.  With  reference  to  the  architecture  in  Fig.  6.3,  the  following 
functions  are  implemented  in  the  kinematics  board: 

•  computation  of  motion  primitives, 

•  computation  of  direct  kinematics,  inverse  kinematics  and  Jacobian, 

•  test  for  trajectory  feasibility, 

•  handling  of  kinematic  redundancy. 

The  dynamics  board  is  devoted  to 

•  computation  of  inverse  dynamics. 

The  servo  board  has  the  functions  of: 

•  microinterpolation  of  references, 

•  computation  of  control  algorithm, 

•  digital-to-analog  conversion  and  interface  with  power  amplifiers, 

•  handling  of  position  and  velocity  transducer  data, 

•  motion  interruption  in  case  of  malfunction. 

The  remaining  boards  in  the  figure  have  been  considered  for  the  sake  of  an 
example  to  illustrate  how  the  use  of  sensors  may  require  local  processing  ca¬ 
pabilities  to  retrieve  significant  information  from  the  given  data  which  can  be 
effectively  used  in  the  sensory  system.  The  force  board  performs  the  following 
operations: 

•  conditioning  of  data  provided  by  the  force  sensor, 

•  representation  of  forces  in  a  given  coordinate  frame. 

The  vision  board  is  in  charge  of: 

•  processing  data  provided  by  the  camera, 

•  extracting  geometric  features  of  the  scene, 

•  localizing  objects  in  given  coordinate  frames. 

Although  the  boards  face  the  same  bus,  the  frequency  at  which  data  are 
exchanged  needs  not  to  be  the  same  for  each  board.  Those  boards  connected 
to  the  proprioceptive  sensors  indeed  need  to  exchange  date  with  the  robot  at 
the  highest  possible  frequency  (from  100  to  1000  Hz)  to  ensure  high  dynamic 
performance  to  motion  control  as  well  as  to  reveal  end-effector  contact  in  a 
very  short  time. 

On  the  other  hand,  the  kinematics  and  dynamics  boards  implement  mod¬ 
elling  functions  and,  as  such,  they  do  not  require  data  update  at  a  rate  as  high 
as  that  required  by  the  servo  board.  In  fact,  manipulator  postures  do  not  vary 
appreciably  in  a  very  short  time,  at  least  with  respect  to  typical  operational 
velocities  and/or  accelerations  of  current  industrial  robots.  Common  sampling 
frequencies  are  in  the  range  of  10  to  100  Hz. 
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Also  the  vision  board  does  not  require  a  high  update  rate,  both  because  the 
scene  is  generally  quasi-static,  and  because  processing  of  interpretive  functions 
are  typically  complex.  Typical  frequencies  are  in  the  range  of  1  to  10  Hz. 

In  summary,  the  board  access  to  the  communication  bus  of  a  hardware 
control  architecture  may  be  performed  according  to  a  multirate  logic  which 
allows  the  solution  of  bus  data  overflow  problems. 
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Problems 

6.1.  With  reference  to  the  situation  illustrated  in  Fig.  6.4,  describe  the  se¬ 
quence  of  actions  required  from  the  manipulator  to  pick  up  an  object  at 
location  A  and  place  it  at  location  B. 

6.2.  For  the  situation  of  Problem  6.1,  find  the  motion  primitives  in  the  cases 
of  given  via  points  and  given  path  points. 

6.3.  The  planar  arm  indicated  in  Fig.  6.5  is  endowed  with  a  wrist  force  sensor 
which  allows  the  measurement  of  the  relevant  force  and  moment  components 
for  the  execution  of  a  peg-in-hole  task.  Draw  the  flow  chart  for  writing  a 
program  to  execute  the  described  task. 

6.4.  A  palletizing  problem  is  represented  in  Fig.  6.6.  Sixteen  equal  objects 
have  to  be  loaded  on  the  pallet.  The  manipulator’s  end-effector  has  to  pick 
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Fig.  6.5.  Peg-in-hole  task 


Fig.  6.6.  Palletizing  task  of  objects  available  on  a  conveyor 


up  the  objects  from  a  conveyor,  whose  feeding  is  commanded  by  the  robot 
in  such  a  way  that  the  objects  are  always  found  in  the  same  location  to  be 
picked.  Write  a  PASCAL  program  to  execute  the  task. 


7 


Dynamics 


Derivation  of  the  dynamic  model  of  a  manipulator  plays  an  important  role 
for  simulation  of  motion,  analysis  of  manipulator  structures,  and  design  of 
control  algorithms.  Simulating  manipulator  motion  allows  control  strategies 
and  motion  planning  techniques  to  be  tested  without  the  need  to  use  a  phys¬ 
ically  available  system.  The  analysis  of  the  dynamic  model  can  be  helpful  for 
mechanical  design  of  prototype  arms.  Computation  of  the  forces  and  torques 
required  for  the  execution  of  typical  motions  provides  useful  information  for 
designing  joints,  transmissions  and  actuators.  The  goal  of  this  chapter  is  to 
present  two  methods  for  derivation  of  the  equations  of  motion  of  a  manipula¬ 
tor  in  the  joint  space.  The  first  method  is  based  on  the  Lagrange  formulation 
and  is  conceptually  simple  and  systematic.  The  second  method  is  based  on  the 
Newton-Euler  formulation  and  yields  the  model  in  a  recursive  form;  it  is  com¬ 
putationally  more  efficient  since  it  exploits  the  typically  open  structure  of  the 
manipulator  kinematic  chain.  Then,  a  technique  for  dynamic  parameter  iden¬ 
tification  is  presented.  Further,  the  problems  of  direct  dynamics  and  inverse 
dynamics  are  formalized,  and  a  technique  for  trajectory  dynamic  scaling  is  in¬ 
troduced,  which  adapts  trajectory  planning  to  the  dynamic  characteristics  of 
the  manipulator.  The  chapter  ends  with  the  derivation  of  the  dynamic  model 
of  a  manipulator  in  the  operational  space  and  the  definition  of  the  dynamic 
manipulability  ellipsoid. 


7.1  Lagrange  Formulation 

The  dynamic  model  of  a  manipulator  provides  a  description  of  the  relationship 
between  the  joint  actuator  torques  and  the  motion  of  the  structure. 

With  Lagrange  formulation ,  the  equations  of  motion  can  be  derived  in 
a  systematic  way  independently  of  the  reference  coordinate  frame.  Once  a 
set  of  variables  qt,  i  =  l,...,n,  termed  generalized  coordinates,  are  chosen 
which  effectively  describe  the  link  positions  of  an  n-DOF  manipulator,  the 
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Lagrangian  of  the  mechanical  system  can  be  defined  as  a  function  of  the 
generalized  coordinates: 

C  =  T~U  (7.1) 

where  T  and  U  respectively  denote  the  total  kinetic  energy  and  potential 
energy  of  the  system. 

The  Lagrange  equations  are  expressed  by 


d_  dc 

dt  dq.i  dq-i 


(7.2) 


where  is  the  generalized  force  associated  with  the  generalized  coordinate  qi- 
Equations  (7.2)  can  be  written  in  compact  form  as 


d_  (dC\T 

dt  \dq  ) 


(7.3) 


where,  for  a  manipulator  with  an  open  kinematic  chain,  the  generalized  coor¬ 
dinates  are  gathered  in  the  vector  of  joint  variables  q.  The  contributions  to 
the  generalized  forces  are  given  by  the  nonconservative  forces,  i.e.,  the  joint 
actuator  torques,  the  joint  friction  torques,  as  well  as  the  joint  torques  induced 
by  end-effector  forces  at  the  contact  with  the  environment.1 

The  equations  in  (7.2)  establish  the  relations  existing  between  the  gener¬ 
alized  forces  applied  to  the  manipulator  and  the  joint  positions,  velocities  and 
accelerations.  Hence,  they  allow  the  derivation  of  the  dynamic  model  of  the 
manipulator  starting  from  the  determination  of  kinetic  energy  and  potential 
energy  of  the  mechanical  system. 


Example  7.1 

In  order  to  understand  the  Lagrange  formulation  technique  for  deriving  the  dynamic 
model,  consider  again  the  simple  case  of  the  pendulum  in  Example  5.1.  With  ref¬ 
erence  to  Fig.  5.8,  let  #  denote  the  angle  with  respect  to  the  reference  position  of 
the  body  hanging  down  (■$  =  0).  By  choosing  d  as  the  generalized  coordinate,  the 
kinetic  energy  of  the  system  is  given  by 

T  =  hti2  +  * Imk2J 2. 

The  system  potential  energy,  defined  at  less  than  a  constant,  is  expressed  by 

U  =  mgt{  1  —  cost?). 

Therefore,  the  Lagrangian  of  the  system  is 

C  =  +  \jlmk2i)2  —  mgl(  1  —  cos-d). 


1  The  term  torque  is  used  as  a  synonym  of  joint  generalized  force. 
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Substituting  this  expression  in  the  Lagrange  equation  in  (7.2)  yields 

(7  +  Imkr )$  +  mg£  sin$  =  £. 

The  generalized  force  £  is  given  by  the  contributions  of  the  actuation  torque  r  at 
the  joint  and  of  the  viscous  friction  torques  —Fd  and  —Frnk^'&,  where  the  latter  has 
been  reported  to  the  joint  side.  Hence,  it  is 

£  =  r  -  Fd  -  Fmkl d 

leading  to  the  complete  dynamic  model  of  the  system  as  the  second-order  differential 
equation 

(7  +  Imkr)d  +  (F  +  Frnk^.)d  +  mg£  sill'd  =  t. 

It  is  easy  to  verify  how  this  equation  is  equivalent  to  (5.29)  when  reported  to  the 
joint  side. 


7.1.1  Computation  of  Kinetic  Energy 

Consider  a  manipulator  with  n  rigid  links.  The  total  kinetic  energy  is  given 
by  the  sum  of  the  contributions  relative  to  the  motion  of  each  link  and  the 
contributions  relative  to  the  motion  of  each  joint  actuator:2 

n 

T  =  Y,(Th+Tmi),  (7.4) 

i—1 

where  Tu  is  the  kinetic  energy  of  Link  i  and  Tmi  is  the  kinetic  energy  of  the 
motor  actuating  Joint  i. 

The  kinetic  energy  contribution  of  Link  i  is  given  by 

%  =  \  I  P*TP*pdV,  (7.5) 

1  Jvti 

where  p*  denotes  the  linear  velocity  vector  and  p  is  the  density  of  the  elemen¬ 
tary  particle  of  volume  dV ;  V(i  is  the  volume  of  Link  i. 

Consider  the  position  vector  p*  of  the  elementary  particle  and  the  position 
vector  pc  of  the  link  centre  of  mass,  both  expressed  in  the  base  frame.  One 
has 

ri  =  [rix  riy  riz  }T  =  p*  -  pu  (7.6) 

with 

Pu  =  —  [  PiPdV  (7.7) 

mu  Jve. 


Link  0  is  fixed  and  thus  gives  no  contribution. 


2 
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Fig.  7.1.  Kinematic  description  of  Link  i  for  Lagrange  formulation 


where  m ti  is  the  link  mass.  As  a  consequence,  the  link  point  velocity  can  be 
expressed  as 

Pi  =  Pt,  +  x  n  (7.8) 

=  Pti  + 

where  p(  is  the  linear  velocity  of  the  centre  of  mass  and  u,  is  the  angular 
velocity  of  the  link  (Fig.  7.1). 

By  substituting  the  velocity  expression  (7.8)  into  (7.5),  it  can  be  recognized 
that  the  kinetic  energy  of  each  link  is  formed  by  the  following  contributions. 

Translational 

The  contribution  is 

If  1 

2  Jv  PliPtiPdV  =  (7-9) 


Mutual 

The  contribution  is 


2  2 


\  Jv  pZSWnpdV^  =2  ^pJ.S(ui)  (p*  -  PfJpdV^ 
since,  by  virtue  of  (7.7),  it  is 

/  P*pdV  =  pli  [  pdV. 

Jv,.  Jv,. 


=  0 
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Rotational 


The  contribution  is 


J  rj ST(ui)S(ui)ripdV  =  ST(ri)S(ri)pdVSj  w; 


where  the  property  S'(a;i)ri  =  — £(rj)u>j  has  been  exploited.  In  view  of  the 
expression  of  the  matrix  operator  S(-) 


SM  = 


o  -nz  riy 


riz 

—n- 


0  -ri: 

T ix  0 


it  is 


The  matrix 


[  rfST(ui)S(u;i)ripdV  =  (7.10) 

Jve  z 


If ,  = 


/ ( riV  +  r1z)pdV  -  f  rixriypdV  -  f  rixrizpdV 


I(rix  +  riz)PdV  - friyrizpdV 

*  f(rix  +  riy)PdV  J 


(7.11) 


Ifixx  Ifixy  Ifi 


IfiVV  dhyz 


is  symmetric3  and  represents  the  inertia  tensor  relative  to  the  centre  of  mass 
of  Link  i  when  expressed  in  the  base  frame.  Notice  that  the  position  of  Link  i 
depends  on  the  manipulator  configuration  and  thus  the  inertia  tensor,  when 
expressed  in  the  base  frame,  is  configuration-dependent.  If  the  angular  velocity 
of  Link  i  is  expressed  with  reference  to  a  frame  attached  to  the  link  (as  in  the 
Denavit-Hartenberg  convention),  it  is 

LO  i  =  7?  (  U); 


where  Rj  is  the  rotation  matrix  from  Link  i  frame  to  the  base  frame.  When 
referred  to  the  link  frame,  the  inertia  tensor  is  constant.  Let  I\.  denote  such 
tensor;  then  it  is  easy  to  verify  the  following  relation: 

Iu  =  R,li  II! .  (7.12) 

If  the  axes  of  Link  i  frame  coincide  with  the  central  axes  of  inertia,  then  the 
inertia  products  are  null  and  the  inertia  tensor  relative  to  the  centre  of  mass 
is  a  diagonal  matrix. 

3  The  symbol  V  has  been  used  to  avoid  rewriting  the  symmetric  elements. 
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By  summing  the  translational  and  rotational  contributions  (7.9)  and  (7.10), 
the  kinetic  energy  of  Link  i  is 

TCi  =  X meipj.pe .  +  (7.13) 

At  this  point,  it  is  necessary  to  express  the  kinetic  energy  as  a  function 
of  the  generalized  coordinates  of  the  system,  that  are  the  joint  variables.  To 
this  end,  the  geometric  method  for  Jacobian  computation  can  be  applied  to 
the  intermediate  link  other  than  the  end-effector,  yielding 

Pit  =  JpiQi  +  ■  ■  ■+Jpl)Qi  =  Jpi]Q  (7-14) 

w*  =  JoiQi  +  ■  ■  -+3oiQi  =  Joi)9,  (7-15) 

where  the  contributions  of  the  Jacobian  columns  relative  to  the  joint  velocities 
have  been  taken  into  account  up  to  current  Link  i.  The  Jacobians  to  consider 
are  then: 


■(b)  _ 

p  ~ 

[&'  ■ 

■■  3 ¥>  0  .. 

■■  o] 

(7.16) 

■(b)  _ 
O  — 

[&  ■ 

••  0 

..  0]; 

(7.17) 

the  columns  of  the  matrices  in  (7.16)  and  (7.17)  can  be  computed  according 
to  (3.30),  giving 


(b)  =  f  z3- 1 

Pj  \  zj- 1  x  (Pi*  -  Pj- 1) 


for  a  prismatic  joint 
for  a  revolute  joint 


(7.18) 


ffd  _  f  0  for  a  prismatic  joint 

X  zj- 1  f°r  a  revolute  joint. 


(7.19) 


where  p  ■_  1  is  the  position  vector  of  the  origin  of  Frame  j  —  1  and  Zj-\  is  the 
unit  vector  of  axis  2  of  Frame  j  —  1.  It  follows  that  the  kinetic  energy  of  Link 
i  in  (7.13)  can  be  written  as 


rr  1  •  T  TUi)T  r 

^ti  —  r>  Q  J  p  J 


p  ]q 


,T  r{li)T 


A  J 


ru:  "!  JyoA- 


(7.20) 


The  kinetic  energy  contribution  of  the  motor  of  Joint  i  can  be  computed 
in  a  formally  analogous  way  to  that  of  the  link.  Consider  the  typical  case  of 
rotary  electric  motors  (that  can  actuate  both  revolute  and  prismatic  joints  by 
means  of  suitable  transmissions).  It  can  be  assumed  that  the  contribution  of 
the  fixed  part  (stator)  is  included  in  that  of  the  link  on  which  such  motor  is 
located,  and  thus  the  sole  contribution  of  the  rotor  is  to  be  computed. 

With  reference  to  Fig.  7.2,  the  motor  of  Joint  i  is  assumed  to  be  located 
on  Link  i  —  1.  In  practice,  in  the  design  of  the  mechanical  structure  of  an  open 
kinematic  chain  manipulator  one  attempts  to  locate  the  motors  as  close  as 
possible  to  the  base  of  the  manipulator  so  as  to  lighten  the  dynamic  load  of 
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the  first  joints  of  the  chain.  The  joint  actuator  torques  are  delivered  by  the 
motors  by  means  of  mechanical  transmissions  (gears).4  The  contribution  of 
the  gears  to  the  kinetic  energy  can  be  suitably  included  in  that  of  the  motor. 
It  is  assumed  that  no  induced  motion  occurs,  i.e. ,  the  motion  of  Joint  i  does 
not  actuate  the  motion  of  other  joints. 

The  kinetic  energy  of  Rotor  i  can  be  written  as 

^iili  ^ mrniPmiPmi  T  ^  ^ ^ )  (7.21) 

where  mmi  is  the  mass  of  the  rotor,  pm.  denotes  the  linear  velocity  of  the 
centre  of  mass  of  the  rotor,  Imi  is  the  inertia  tensor  of  the  rotor  relative  to 
its  centre  of  mass,  and  LOmi  denotes  the  angular  velocity  of  the  rotor. 

Let  dmi  denote  the  angular  position  of  the  rotor.  On  the  assumption  of  a 
rigid  transmission ,  one  has 

kri<ii  =  firm  (7.22) 

where  kri  is  the  gear  reduction  ratio.  Notice  that,  in  the  case  of  actuation  of 
a  prismatic  joint,  the  gear  reduction  ratio  is  a  dimensional  quantity. 

According  to  the  angular  velocity  composition  rule  (3.18)  and  the  rela¬ 
tion  (7.22),  the  total  angular  velocity  of  the  rotor  is 

=  OJj  —  i  T  kriQiZrni  (7.23) 

where  is  the  angular  velocity  of  Link  i  —  1  on  which  the  motor  is  located, 
and  zmi  denotes  the  unit  vector  along  the  rotor  axis. 

4  Alternatively,  the  joints  may  be  actuated  by  torque  motors  directly  coupled  to 
the  rotation  axis  without  gears. 
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To  express  the  rotor  kinetic  energy  as  a  function  of  the  joint  variables,  it 
is  worth  expressing  the  linear  velocity  of  the  rotor  centre  of  mass  —  similarly 
to  (7.14)  —  as 

pmi  =  J^q.  (7.24) 

The  Jacobian  to  compute  is  then 

4roi)  =  [j^*)  0  °]  (7-25) 

whose  columns  are  given  by 

(m4)  _  f  Zj-i  for  a  prismatic  joint 

■*pi  ~~  1  Zj- 1  x  (pm.  —  Pj-i)  for  a  revolute  joint  '  '  ' 

where  Pj-i  is  the  position  vector  of  the  origin  of  Frame  j  —  1.  Notice  that 

Pp-  ’  =  0  in  (7.25),  since  the  centre  of  mass  of  the  rotor  has  been  taken  along 
its  axis  of  rotation. 


The  angular  velocity  in  (7.23)  can  be  expressed  as  a  function  of  the  joint 
variables,  i.e., 


r(mi)  • 

—  *7(9  9- 

(7.27) 

The  Jacobian  to  compute  is  then 

j(mi)  __  r  (to4) 

Jo  -  LJoi 

0  o] 

(7.28) 

whose  columns,  in  view  of  (7.23), 

(7.15),  are  respectively  given  by 

>*)  _  )  JOj 

J°3  ~  \  , 

j  =  l,...,*-l 

(7.29) 

1  KriZrn: 

i  =  *■ 

To  compute  the  second  relation  in  (7.29),  it  is  sufficient  to  know  the  compo¬ 
nents  of  the  unit  vector  of  the  rotor  rotation  axis  zmi  with  respect  to  the  base 
frame.  Hence,  the  kinetic  energy  of  Rotor  i  can  be  written  as 


—  2 


1 


for  t Ntr 

2  q  J  o 


(mi) 


q.  (7.30) 


Finally,  by  summing  the  various  contributions  relative  to  the  single  links 
(7.20)  and  single  rotors  (7.30)  as  in  (7.4),  the  total  kinetic  energy  of  the 
manipulator  with  actuators  is  given  by  the  quadratic  form 

1  n  n  1 

r  =  2  ^  E  b(q)<i  (7-31) 

i= 1  i= 1 


where 


n 

B(q)  =  E  ( 


rri£.J 


(U)T  T(U) 
p  up 


f{U)T 


JD  ji  tdT 


(7.32) 


i= 1 


H-772m.«/  r 


dr 


j; 


D  Trrli  Di  T 

-£Xmz -*•  rrii  Jrvmi  u  O 


(mi) 


is  the  (n  x  n)  inertia  matrix  which  is: 
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•  symmetric, 

•  positive  definite, 

•  (in  general)  configuration- dependent. 

7.1.2  Computation  of  Potential  Energy 

As  done  for  kinetic  energy,  the  potential  energy  stored  in  the  manipulator  is 
given  by  the  sum  of  the  contributions  relative  to  each  link  as  well  as  to  each 
rotor: 

n 

W  =  ]T(Z4+Z 40-  (7-33) 

i= 1 

On  the  assumption  of  rigid  links,  the  contribution  due  only  to  gravitational 
forces5  is  expressed  by 

Uu  =  ~  [  QoPiPdV  =  -me^oPe.  (7.34) 

Jvh 

where  g0  is  the  gravity  acceleration  vector  in  the  base  frame  (e.g.,  gQ  = 
[0  0  —  g]T  if  £  is  the  vertical  axis),  and  (7.7)  has  been  utilized  for  the 

coordinates  of  the  centre  of  mass  of  Link  i.  As  regards  the  contribution  of 
Rotor  i,  similarly  to  (7.34),  one  has 

Idrrii  —  (7.35) 

By  substituting  (7.34),  (7.35)  into  (7.33),  the  potential  energy  is  given  by 

n 

U=~  ^2{meigoP(.  +  rnmiglpm.)  (7.36) 

i= 1 

which  reveals  that  potential  energy,  through  the  vectors  pe.  and  pm.  is  a 
function  only  of  the  joint  variables  q,  and  not  of  the  joint  velocities  q. 


7.1.3  Equations  of  Motion 

Having  computed  the  total  kinetic  and  potential  energy  of  the  system  as 
in  (7.31),  (7.36),  the  Lagrangian  (7.1)  for  the  manipulator  can  be  written  as 

C(q,q)  =  T{q,q)-U{q).  (7.37) 

Taking  the  derivatives  required  by  Lagrange  equations  in  (7.3)  and  recalling 
that  U  does  not  depend  on  q  yields 

B{q)q  +  n(q,q)  =  £  (7.38) 

5  In  the  case  of  link  flexibility,  one  would  have  an  additional  contribution  due  to 
elastic  forces. 
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where 

"<«■  «>  =  -  Ks (i> TB(q)i‘ ])  +  (^r)  • 

In  detail,  noticing  that  U  in  (7.36)  does  not  depend  on  q  and  accounting 
for  (7.31)  yields 


d  (dC 


d  ( dT 


dbij(q) 


j= i 


i=i 

n  n 


dt 


-9j 


3= 1 


j=l  fc=l 


gMg) 

dqk 


-Qkqj 


and 


dT  1  ^  dbjk{q) 


i=i  fc=i 


3gi 


where  the  indices  of  summation  have  been  conveniently  switched.  Further,  in 
view  of  (7.14),  (7.24),  it  is 


07 


dp^ 


dqi 


=  {meiqo  3{m  (q)  +  mmjgo  Jnj\q))  =  &(<?) 

j= i 


where,  again,  the  index  of  summation  has  been  changed. 
As  a  result,  the  equations  of  motion  are 


(7.39) 


n  n  n 

E6*i(«)®  +  EE  hijk{q)qkqj  +  gi{q)  =  &  i  =  1, . . . ,  n 

j=i  j=i  fc=i 

where 

dbij  1  dbjk 

ljk  dqk  2  ‘ 

A  physical  interpretation  of  (7.40)  reveals  that: 


(7.40) 


(7.41) 


•  For  the  acceleration  terms: 

The  coefficient  represents  the  moment  of  inertia  at  Joint  i  axis, 
in  the  current  manipulator  configuration,  when  the  other  joints  are 
blocked. 

The  coefficient  btJ  accounts  for  the  effect  of  acceleration  of  Joint  j  on 
Joint  j. 

•  For  the  quadratic  velocity  terms: 

The  term  hijjqj  represents  the  centrifugal  effect  induced  on  Joint  i  by 
velocity  of  Joint  j;  notice  that  hm  =  0,  since  dbu/dqi  =  0. 
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The  term  hijkqjqk  represents  the  Coriolis  effect  induced  on  Joint  i  by 
velocities  of  Joints  j  and  k. 

•  For  the  configuration- dependent  terms: 

The  term  gi  represents  the  moment  generated  at  Joint  i  axis  of  the 
manipulator,  in  the  current  configuration,  by  the  presence  of  gravity. 

Some  joint  dynamic  couplings,  e.g.,  coefficients  bi7  and  may  be  re¬ 
duced  or  zeroed  when  designing  the  structure,  so  as  to  simplify  the  control 
problem. 

Regarding  the  nonconservative  forces  doing  work  at  the  manipulator  joints, 
these  are  given  by  the  actuation  torques  t  minus  the  viscous  friction  torques 
Fvq  and  the  static  friction  torques  f  s{q ,  q):  Fv  denotes  the  (n  x  n)  diagonal 
matrix  of  viscous  friction  coefficients.  As  a  simplified  model  of  static  friction 
torques,  one  may  consider  the  Coulomb  friction  torques  Fs  sgn  (q),  where  Fs 
is  an  {n  x  n)  diagonal  matrix  and  sgn  (q)  denotes  the  (n  x  1)  vector  whose 
components  are  given  by  the  sign  functions  of  the  single  joint  velocities. 

If  the  manipulator’s  end-effector  is  in  contact  with  an  environment,  a 
portion  of  the  actuation  torques  is  used  to  balance  the  torques  induced  at 
the  joints  by  the  contact  forces.  According  to  a  relation  formally  analogous 
to  (3.111),  such  torques  are  given  by  JT(q)he  where  he  denotes  the  vector  of 
force  and  moment  exerted  by  the  end-effector  on  the  environment. 

In  summary,  the  equations  of  motion  (7.38)  can  be  rewritten  in  the  com¬ 
pact  matrix  form  which  represents  the  joint  space  dynamic  model: 

B{q)q  +  C(q,  q)q  +  Fvq  +  Fs  sgn  (q)  +  g{q)  =  r  -  JT(q)he  (7.42) 

where  C  is  a  suitable  (n  x  n)  matrix  such  that  its  elements  c-ij  satisfy  the 
equation 

n  n  n 

Y  c^:>  =  YY  hijkqkqj-  (7.43) 

j= i  j= i fc= l 

7.2  Notable  Properties  of  Dynamic  Model 

In  the  following,  two  notable  properties  of  the  dynamic  model  are  presented 
which  will  be  useful  for  dynamic  parameter  identification  as  well  as  for  deriving 
control  algorithms. 


7.2.1  Skew-symmetry  of  Matrix  B  —  2 C 

The  choice  of  the  matrix  C  is  not  unique,  since  there  exist  several  matri¬ 
ces  C  whose  elements  satisfy  (7.43).  A  particular  choice  can  be  obtained  by 
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elaborating  the  term  on  the  right-hand  side  of  (7.43)  and  accounting  for  the 
expressions  of  the  coefficients  h-Ljk  in  (7.41).  To  this  end,  one  has 


I =  EE  hijkQkQj 

3= 1  3  =  1  k= 1 


-EE 

j— i  fe= i 


/  56^  _  1  dbjk  \  .  . 


\dqk  2  dqi 


Qkqj- 


Splitting  the  first  term  on  the  right-hand  side  by  an  opportune  switch  of 
summation  between  j  and  k  yields 


n  n 

obi 


Ej-  \  '  \  '  uuij  .  .  1  \  -  \  -  ( dbik 

Cijqj  iEE +  2 E E {-^7 

j=i  j= l  &=i  j= l 


As  a  consequence,  the  generic  element  of  C  is 


n 

Cij  —  ^  ^  Cijk.qk 
fc= 1 


dbjk  \ 
dqi  ) 


ikij- 


(7.44) 


where  the  coefficients 

_  1  f  dbij_  dbik  _  dbjk\ 
C‘:,k  2  \dqk  dqj  dqi  J 


(7.45) 


are  termed  Christoffel  symbols  of  the  first  type.  Notice  that,  in  view  of  the 
symmetry  of  B,  it  is 

C-ijk  Cikj-  (746) 

This  choice  for  the  matrix  C  leads  to  deriving  the  following  notable  prop¬ 
erty  of  the  equations  of  motion  (7.42).  The  matrix 


N(q,q)  =  B(q)-2C(q,q)  (7.47) 


is  skew- symmetric;  that  is,  given  any  (n  x  1)  vector  w,  the  following  relation 
holds: 

wTN(q,  q)w  =  0.  (7.48) 

In  fact,  substituting  the  coefficient  (7.45)  into  (7.44)  gives 


and  then  the  expression  of  the  generic  element  of  the  matrix  N  in  (7.47)  is 
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The  result  follows  by  observing  that 

Tlij  —  Tlji- 


An  interesting  property  which  is  a  direct  implication  of  the  skew-symmetry 
of  N(q,  q)  is  that,  by  setting  w  =  q, 

qTN(q,q)q  =  0-  (7.49) 

notice  that  (7.49)  does  not  imply  (7.48),  since  N  is  a  function  of  q,  too. 

It  can  be  shown  that  (7.49)  holds  for  any  choice  of  the  matrix  C,  since  it 
is  a  result  of  the  principle  of  conservation  of  energy  (Hamilton).  By  virtue  of 
this  principle,  the  total  time  derivative  of  kinetic  energy  is  balanced  by  the 
power  generated  by  all  the  forces  acting  on  the  manipulator  joints.  For  the 
mechanical  system  at  issue,  one  may  write 

7,  ^  ( <iTB(q)q )  =  q1  (r  -  Fvq  -  Fs  sgn  (q)  -  g(q)  -  JT(q)he) .  (7.50) 

Taking  the  derivative  on  the  left-hand  side  of  (7.50)  gives 

\qT B(q)q+  qT  B(q)q 

and  substituting  the  expression  of  B(q)q  in  (7.42)  yields 

^(V  -B(q)q)  =  \qr(B(q)  -  2 C(q,  q))q  (7.51) 

+Q 1  (t  -  Fvq  -  Fs  sgn  (q)  -  g(q)  -  JT (q)he) . 

A  direct  comparison  of  the  right-hand  sides  of  (7.50)  and  (7.51)  leads  to  the 
result  established  by  (7.49). 

To  summarize,  the  relation  (7.49)  holds  for  any  choice  of  the  matrix  C, 
since  it  is  a  direct  consequence  of  the  physical  properties  of  the  system, 
whereas  the  relation  (7.48)  holds  only  for  the  particular  choice  of  the  ele¬ 
ments  of  C  as  in  (7.44),  (7.45). 


7.2.2  Linearity  in  the  Dynamic  Parameters 

An  important  property  of  the  dynamic  model  is  the  linearity  with  respect  to 
the  dynamic  parameters  characterizing  the  manipulator  links  and  rotors. 

In  order  to  determine  such  parameters,  it  is  worth  associating  the  kinetic 
and  potential  energy  contributions  of  each  rotor  with  those  of  the  link  on 
which  it  is  located.  Hence,  by  considering  the  union  of  Link  i  and  Rotor  i  +  1 
( augmented  Link  i),  the  kinetic  energy  contribution  is  given  by 

T%  —  +  Tmi+ 1 


(7.52) 
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where 

Tu  =  2  muPliPti  +  2  (7.53) 

and 

Trrii+\  2 mrni+iPmi+1Pmi+1  "b  2^rrhi+i^'mi+1^rrhi+1‘  (7-54) 

With  reference  to  the  centre  of  mass  of  the  augmented  link,  the  linear  velocities 
of  the  link  and  rotor  can  be  expressed  according  to  (3.26)  as 

Pti  =  PCi  +  Ui  X  rCi,tt  (7.55) 

Pmi+1  =  PC,  +  W*  X  rCi,mi+1  (7.56) 

with 

rctA  =  Pt„  ~  Pc,.  (7-57) 

rc»,mi+1  =  pmi+1  -  pCi,  (7.58) 

where  pc  denotes  the  position  vector  of  the  centre  of  mass  of  augmented 
Link  i. 

Substituting  (7.55)  into  (7.53)  gives 

Tti  =  ^muPciPat  +Pcis(UJi)murci,u  (7-59) 

+  ^mUu>iST(rCi,u)S(rci,u)(j}i  +  <°i- 

By  virtue  of  Steiner  theorem,  the  matrix 

iu  =  lu  +  mhST(rciA)S(rCiA)  (7-60) 

represents  the  inertia  tensor  relative  to  the  overall  centre  of  mass  pc. ,  which 
contains  an  additional  contribution  due  to  the  translation  of  the  pole  with 
respect  to  which  the  tensor  is  evaluated,  as  in  (7.57).  Therefore,  (7.59)  can  be 
written  as 


Tu  =  ^ rnuPciPCi  +Pcis(u,i)mUrCi,u  +  (7.61) 

In  a  similar  fashion,  substituting  (7.56)  into  (7.54)  and  exploiting  (7.23) 
yields 

Tmi+ 1  2  ^ m i+lP C, P C’i  PCi  T Ci,rrii+\  T  i  Irrii+i^i  (7.62) 

+kr,i+lQi+lZmi+1Imi+1u,i  +  2^r,i+l^i+lZmi+1^mi+1zm.i+ii 


^mi+l  ^mi+ 1  +  mm  ST(rCi  ,mi+ 1  )S(rCi  ,mi+ 1)- 


where 


(7.63) 
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Summing  the  contributions  in  (7.61),  (7.62)  as  in  (7.52)  gives  the  expres¬ 
sion  of  the  kinetic  energy  of  augmented  Link  i  in  the  form 


T~i  —  —mip^.pQ.  +  zizjf  li^i  +  fcr,i+l4i+l'Z 


mj+prai+i"! 


<->  ^Tyi+lQi+l* 


'mill  m'+i' 


(7.64) 


where  m,  =  +  mmi+1  and  /,  =  I(.  +  Imi+1  are  respectively  the  overall 

mass  and  inertia  tensor.  In  deriving  (7.64),  the  relations  in  (7.57),  (7.58)  have 
been  utilized  as  well  as  the  following  relation  between  the  positions  of  the 
centres  of  mass: 

mUPli  +  mmi+1Pmi+1  =  miPCi ■  (7-65) 

Notice  that  the  first  two  terms  on  the  right-hand  side  of  (7.64)  represent 
the  kinetic  energy  contribution  of  the  rotor  when  this  is  still,  whereas  the 
remaining  two  terms  account  for  the  rotor’s  own  motion. 

On  the  assumption  that  the  rotor  has  a  symmetric  mass  distribution  about 
its  axis  of  rotation,  its  inertia  tensor  expressed  in  a  frame  Rmi  with  origin  at 
the  centre  of  mass  and  axis  zmi  aligned  with  the  rotation  axis  can  be  written 
as 

IrriiXX  0 

jm-i  —  n  T 

±mi~  u  1miyy 

0  0 


(7.66) 


where  Imiyy  =  Irmxx-  As  a  consequence,  the  inertia  tensor  is  invariant  with 
respect  to  any  rotation  about  axis  zmi  and  is,  anyhow,  constant  when  referred 
to  any  frame  attached  to  Link  i  —  1. 

Since  the  aim  is  to  determine  a  set  of  dynamic  parameters  independent  of 
the  manipulator  joint  configuration,  it  is  worth  referring  the  inertia  tensor  of 
the  link  Ii  to  frame  Ri  attached  to  the  link  and  the  inertia  tensor  Imi+1  to 
frame  Rm  so  that  it  is  diagonal.  In  view  of  (7.66)  one  has 


lmi+izmi+i  —  ■n-mi+1 1  mi+1 


(7.67) 


where  Imi+1  =  Imi+1zz  denotes  the  constant  scalar  moment  of  inertia  of  the 
rotor  about  its  rotation  axis. 

Therefore,  the  kinetic  energy  (7.64)  becomes 


%  =  -miPcftCi 


iuiT~Tl 
2  1 


iT 


+  ^r,i+l<ji+lzmi+1Zmi+1 


U>i 


(7.68) 


According  to  the  linear  velocity  composition  rule  for  Link  i  in  (3.15),  one 
may  write 

PCi  =  Pi  +  ui  x  (7.69) 


262  7  Dynamics 


where  all  the  vectors  have  been  referred  to  Frame  i:  note  that  r\  c.  is  fixed  in 
such  a  frame.  Substituting  (7.69)  into  (7.68)  gives 


%  =  \™iP? Pl+Pf  S(vl)mirlc.  + 


(7.70) 


+  &r,i+l<3,i+lJri 


ziT 

mi+l  ^mi+ 


1W!  +  2^r’i+1<^i+1^mi+1’ 


where 

Ti  =  ii  +  mtST(rlcJS(rlCi)  (7.71) 

represents  the  inertia  tensor  with  respect  to  the  origin  of  Frame  i  according 
to  Steiner  theorem. 

Let  r\  c.  =  \  (-CiX  £ £ CiZ  ]T-  The  first  moment  of  inertia  is 


miri,Ci 


( l/.j  t'  Cr  y 

mddz 


(7.72) 


From  (7.71)  the  inertia  tensor  of  augmented  Link  i  is 


~fixx  T  UO  ( £(ji  y  -\  £ QiZ )  fixy  UJ i^CiX^-Ciy  fixz  iHif-C.x^C.z 

*  fiyy  T  mi{£‘cix  T  ^Ciz)  ~fiyz  ~  mi£Ciy£CiZ 

*  *  hzz  +  mi{^CiX  +  ^ Ciyl 


* 


* 


fixy 

fiyy 

* 


(7.73) 


Therefore,  the  kinetic  energy  of  the  augmented  link  is  linear  with  respect  to 
the  dynamic  parameters,  namely,  the  mass,  the  three  components  of  the  first 
moment  of  inertia  in  (7.72),  the  six  components  of  the  inertia  tensor  in  (7.73), 
and  the  moment  of  inertia  of  the  rotor. 

As  regards  potential  energy,  it  is  worth  referring  to  the  centre  of  mass  of 
augmented  Link  i  defined  as  in  (7.65),  and  thus  the  single  contribution  of 
potential  energy  can  be  written  as 


Hi  =  -migtQplCi  (7.74) 

where  the  vectors  have  been  referred  to  Frame  i.  According  to  the  relation 

PCi  =  Pi  +  rl,Ci- 

The  expression  in  (7.74)  can  be  rewritten  as 

Ui  =  -gl0T  ( mipl  +  mirliC.)  (7.75) 
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that  is,  the  potential  energy  of  the  augmented  link  is  linear  with  respect  to 
the  mass  and  the  three  components  of  the  first  moment  of  inertia  in  (7.72). 

By  summing  the  contributions  of  kinetic  energy  and  potential  energy  for 
all  augmented  links,  the  Lagrangian  of  the  system  (7.1)  can  be  expressed  in 
the  form 

n 

£  =  YJ&i  -  PuiUi  (7.76) 

2=1 

where  7Tj  is  the  (11  x  1)  vector  of  dynamic  parameters 


^  i  \  Up  llli  tCi x  tfl'i  tc,y  'W'i^CiZ  I'/xx  I ixy  d'ixz  I iyy  ^iyz  ^izz  ^rn;  ]  ? 

(7.77) 

in  which  the  moment  of  inertia  of  Rotor  i  has  been  associated  with  the  pa¬ 
rameters  of  Link  i  so  as  to  simplify  the  notation. 

In  (7.76),  (3Ti  and  /3Ui  are  two  (11  x  1)  vectors  that  allow  the  La¬ 
grangian  to  be  written  as  a  function  of  7 t*.  Such  vectors  are  a  function 
of  the  generalized  coordinates  of  the  mechanical  system  (and  also  of  their 
derivatives  as  regards  /3rJ.  In  particular,  it  can  be  shown  that  (3Ti  = 
•  •  • ,  qi,  qi,  92,  •  •  • ,  <ji)  and  f3Ui  =  Pui{qi,q2,  ■  ■■,  qi),  he.,  they  do  not 
depend  on  the  variables  of  the  joints  subsequent  to  Link  i. 

At  this  point,  it  should  be  observed  how  the  derivations  required  by  the 
Lagrange  equations  in  (7.2)  do  not  alter  the  property  of  linearity  in  the  pa¬ 
rameters,  and  then  the  generalized  force  at  Joint  i  can  be  written  as 

n 

e,'  (7.78) 

i= i 


where 


_  d  d@Tj  dfluj 

Vn  dt  dqi  dqt  dqt 


(7.79) 


Since  the  partial  derivatives  of  /3Tj  and  f3Uj  appearing  in  (7.79)  vanish  for 
j  <  i,  the  following  notable  result  is  obtained: 


61 

6 

'2/ii  2/12  • 
oT  yTn  ■ 

• 

_ 1 

.  ^  ^ 

tO  h-1 

_ I 

sn  .  . 

3 

.  0T  0T  . 

T 

•  •  y nn  - 

- 

(7.80) 


which  yields  the  property  of  linearity  of  the  model  of  a  manipulator  with 
respect  to  a  suitable  set  of  dynamic  parameters . 

In  the  simple  case  of  no  contact  forces  (he  =  0),  it  may  be  worth  including 
the  viscous  friction  coefficient  FVi  and  Coulomb  friction  coefficient  FSi  in  the 
parameters  of  the  vector  7T,; ,  thus  leading  to  a  total  number  of  13  parameters 
per  joint.  In  summary,  (7.80)  can  be  compactly  written  as 


t  =  Y(q,q,q)  tv 


(7.81) 
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Fig.  7.3.  Two-link  Cartesian  arm 

where  tv  is  a  {p  x  1)  vector  of  constant  parameters  and  Y  is  an  (n  x  p )  matrix 
which  is  a  function  of  joint  positions,  velocities  and  accelerations ;  this  matrix 
is  usually  called  regressor.  Regarding  the  dimension  of  the  parameter  vector, 
notice  that  p  <  13?i  since  not  all  the  thirteen  parameters  for  each  joint  may 
explicitly  appear  in  (7.81). 

7.3  Dynamic  Model  of  Simple  Manipulator  Structures 

In  the  following,  three  examples  of  dynamic  model  computation  are  illustrated 
for  simple  two-DOF  manipulator  structures.  Two  DOFs,  in  fact,  are  enough 
to  understand  the  physical  meaning  of  all  dynamic  terms,  especially  the  joint 
coupling  terms.  On  the  other  hand,  dynamic  model  computation  for  manip¬ 
ulators  with  more  DOFs  would  be  quite  tedious  and  prone  to  errors,  when 
carried  out  by  paper  and  pencil.  In  those  cases,  it  is  advisable  to  perform  it 
with  the  aid  of  a  symbolic  programming  software  package. 

7.3.1  Two-link  Cartesian  Arm 

Consider  the  two-link  Cartesian  arm  in  Fig.  7.3,  for  which  the  vector  of  gen¬ 
eralized  coordinates  is  q  =  [di  ■  Let  mq ,  me2  be  the  masses  of  the  two 

links,  and  mmi,  mTO2  the  masses  of  the  rotors  of  the  two  joint  motors.  Also  let 
Imi ,  Im2  be  the  moments  of  inertia  with  respect  to  the  axes  of  the  two  rotors. 
It  is  assumed  that  pm.  =  p7 _ ,  and  zrrli  =  Zj_i,  for  i  =  1,2,  i.e.,  the  motors 
are  located  on  the  joint  axes  with  centres  of  mass  located  at  the  origins  of  the 
respective  frames. 

With  the  chosen  coordinate  frames,  computation  of  the  Jacobians  in  (7.16), 
(7.18)  yields 

To  ol  To  r 

J{pl]  =00  J{p2)  =00. 

i  oj  L1  ° 
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Obviously,  in  this  case  there  are  no 
links. 

Computation  of  the  Jacobians  in 


angular  velocity  contributions  for  both 
(7.25),  (7.26)  e  (7.28),  (7.29)  yields 


'0 

0 

o' 

0 

rim 2)  _ 

'0 

0 

0 

0 

U  p 

0 

0  1  0 

'  0 

o' 

'0 

kr2 

0 

0 

j(m2) 

J  0  — 

0 

0 

?- 

-se 

_ 1 

0 

0 

0 

where  kri  is  the  gear  reduction  ratio  of  Motor  i.  It  is  obvious  to  see  that 
z\  =  [1  0  0]T,  which  greatly  simplifies  computation  of  the  second  term 

in  (7.30). 

From  (7.32),  the  inertia  matrix  is 


B  = 


mtl 


mn 


k2  T 

fi-rl  In 


mt2 


771^2  “1“  kr2lrri2 


It  is  worth  observing  that  B  is  constant ,  i.e.,  it  does  not  depend  on  the  arm 
configuration.  This  implies  also  that  C  =  O,  i.e.,  there  are  no  contributions 
of  centrifugal  and  Coriolis  forces.  As  for  the  gravitational  terms,  since  gQ  = 
[0  0  —  g]T  (g  is  gravity  acceleration),  (7.39)  with  the  above  Jacobians  gives 


gi  =  (mei  +  m.m2  +  me2)g  g2  =  0. 


In  the  absence  of  friction  and  tip  contact  forces,  the  resulting  equations  of 
motion  are 


(m<q  +  m.m2  +  +  me2)di  +  (■ me1  +  mm2  +  me2)g  =  n 

{mi2  +  k^2lm2)d  2  =  T2 

where  T\  and  r2  denote  the  forces  applied  to  the  two  joints.  Notice  that  a 
completely  decoupled  dynamics  has  been  obtained.  This  is  a  consequence  not 
only  of  the  Cartesian  structures  but  also  of  the  particular  geometry;  in  other 
words,  if  the  second  joint  axis  were  not  at  a  right  angle  with  the  first  joint 
axis,  the  resulting  inertia  matrix  would  not  be  diagonal  (see  Problem  7.1). 


7.3.2  Two-link  Planar  Arm 

Consider  the  two-link  planar  arm  in  Fig.  7.4,  for  which  the  vector  of  general¬ 
ized  coordinates  is  q  =  [di  7?2]T-  Let  l\,  t2  be  the  distances  of  the  centres 
of  mass  of  the  two  links  from  the  respective  joint  axes.  Also  let  rrit1 ,  me2  be 
the  masses  of  the  two  links,  and  mmi ,  mm2  the  masses  of  the  rotors  of  the  two 
joint  motors.  Finally,  let  Imi,  Im2  be  the  moments  of  inertia  with  respect  to 
the  axes  of  the  two  rotors,  and  7^,  Ii2  the  moments  of  inertia  relative  to  the 
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Fig.  7.4.  Two-link  planar  arm 


centres  of  mass  of  the  two  links,  respectively.  It  is  assumed  that  pm  —  pt_i 
and  zmi  =  Zj_i,  for  i  =  1,2,  i.e.,  the  motors  are  located  on  the  joint  axes 
with  centres  of  mass  located  at  the  origins  of  the  respective  frames. 

With  the  chosen  coordinate  frames,  computation  of  the  Jacobians  in  (7.16), 
(7.18)  yields 


—VlSl 

O' 

r(4) 

Up  — 

~  «lSl  — 

-^2«12 

hci 

0 

OlCl  +  ^2^12 

(-2C12 

0 

0 

0 

0 

whereas  computation  of  the  Jacobians  in  (7.17),  (7.19)  yields 


'0 

o' 

II 

'0 

o' 

0 

0 

0 

0 

1 

0 

1 

1 

Notice  that  tUj,  for  i  =  1,2,  is  aligned  with  Zo,  and  thus  -R;  has  no  effect.  It 
is  then  possible  to  refer  to  the  scalar  moments  of  inertia  I(t . 

Computation  of  the  Jacobians  in  (7.25),  (7.26)  yields 


ro 

01 

~a1s1 

01 

t(”u)  _ 

J  p  — 

1 

0  0 

1 

0  0 
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J  p  — 
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whereas  computation  of  the  Jacobians  in  (7.28),  (7.29)  yields 
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— 1 
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where  kri  is  the  gear  reduction  ratio  of  Motor  i. 
From  (7.32),  the  inertia  matrix  is 


B(q) 


bu  ($2)  ^12(^2) 

&21  ($2)  b22 
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bn  —  Ie1  +  +  k^ilmi  +  It2  +  m£2(ai  +  +  201/202) 

d“7m2  d“  tttm2 0] 

bl2  =  &21  =  ^2  +  mi2{^2  +  a1^2C2)  +  kr2lm2 
&22  =  +  m^2^2  +  k^Im2- 


Compared  to  the  previous  example,  the  inertia  matrix  is  now  configuration- 
dependent.  Notice  that  the  term  fer2^m2  in  the  off-diagonal  term  of  the  inertia 
matrix  derives  from  having  considered  the  rotational  part  of  the  motor  ki¬ 
netic  energy  as  due  to  the  total  angular  velocity,  i.e.,  its  own  angular  velocity 
and  that  of  the  preceding  link  in  the  kinematic  chain.  At  first  approximation, 
especially  in  the  case  of  high  values  of  the  gear  reduction  ratio,  this  contribu¬ 
tion  could  be  neglected;  in  the  resulting  reduced  model,  motor  inertias  would 
appear  uniquely  in  the  elements  on  the  diagonal  of  the  inertia  matrix  with 
terms  of  the  type  k^I^. 

The  computation  of  Christoffel  symbols  as  in  (7.45)  gives 


1  dbn  n 
c  111  —  x  -j) —  —  0 

2  dqx 

1  dbn  _  „  , 

C112  —  C121  —  —  —  —rnt2a\i2S2  —  h 


C122  = 


C211  = 


db. 


2  dq2 
1  <%2 


dq2 

db2i 


2  dqi 

_ 1  dbn 

dqi  2  dq2 


=  h 
=  —h 


1  <97*22  n 
C212  —  C221  —  -  -w —  =  o 

2  dqi 

1  db22  „ 

C222  —  x  —  —  0, 


2  dq2 


leading  to  the  matrix 


C(q,q)  = 


hi) 2  h(i)  1  +  i)2) 

—hi)  1  0 


Computing  the  matrix  N  in  (7.47)  gives 
N(q,q)  =  B(q)-2C(q,q) 

'  2 hi) 2  hi) 2 


-2 


hi)  2  0 

0  —2hi)\  —  hi)2 

2hi)\  +  hi)  2  0 


M2  h{i)  1  +  i)2) 

—hi)  1  0 


that  allows  the  verification  of  the  skew-symmetry  property  expressed  by  (7.48). 
See  also  Problem  7.2. 
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As  for  the  gravitational  terms,  since  g0  =  [0  —g  0]T,  (7.39)  with  the 
above  Jacobians  gives 

g\  =  (mej  i  +  mm2ai  +  me2ai)gci  +  m.ej2gc12 
32  =  rne2£2gci2. 

In  the  absence  of  friction  and  tip  contact  forces,  the  resulting  equations  of 
motion  are 

{it  i  +  +  k~1Imi  +  I(2  +  +  2ai£2c2)  +  I  m2  ^m2®l)  ^1 

+  {Iz2  +  rri£2  (£2  +  C2)  +  k  r2  I  m2  )  ^2 

— 2m£2ai^2S2i?i^2  —  m£2ai£2S2^2 

+  +  mm2ai  +  m^a^gcx  +  mi2£2gc12  =  n  (7.82) 

(le2  +  me2(el  +  ai£2c2)  +  k 

r2^m2  )  +  ( I(2  +  mt2£\  +  k^2Im2)  $2 

+me,2a1i2s2'&\  +  me2£2gc12  =  r2 


where  t\  and  r2  denote  the  torques  applied  to  the  joints. 

Finally,  it  is  wished  to  derive  a  parameterization  of  the  dynamic  model 
(7.82)  according  to  the  relation  (7.81).  By  direct  inspection  of  the  expressions 
of  the  joint  torques,  it  is  possible  to  find  the  following  parameter  vector: 

7T=[7Ti  7 r2  7 r3  7T4  7T5  7T6  7T7  7T8  }T  (7.83) 


7Ti  =  mi  =  me1  +  mm2 

7T2  =  TOi^Ci  =  "hh(^L  -  ai) 

7T.3  =  I\  =  +  Wifi  (^1  —  «l)“  +  ^m2 

774  —  Imi 

7T.5  =m2  =  me2 

7t6  =  m2£c2  =  mi2  (i2  -  a2) 

7t7  =  /2  =  Ii2  +  m<2(f2  —  a2)2 
778  =  An2  ? 

where  the  parameters  for  the  augmented  links  have  been  found  according 
to  (7.77).  It  can  be  recognized  that  the  number  of  non-null  parameters  is  less 
than  the  maximum  number  of  twenty-two  parameters  allowed  in  this  case.6 
The  regressor  in  (7.81)  is 


Y  = 


y  11 

2/21 


2/12  y  13  y 14  2/15  2/16  2/17  318 

322  323  324  325  326  327  328 


(7.84) 


The  number  of  parameters  can  be  further  reduced  by  resorting  to  a  more  accurate 
inspection,  which  leads  to  finding  a  minimum  number  of  five  parameters;  those 
turn  out  to  be  a  linear  combination  of  the  parameters  in  (7.83)  (see  Problem  7.4). 
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2/n  =  alti  i  +  aigci 
2/12  =  2oii?i  +  gci 
2/13  =  $1 
2/14  =  kl  t'!?! 

1/15  =  (a?  +  2aia2c2  +  a2)$i  +  (aia2c2  +  a\)&2  —  2aia2s2t?i$2 
-aia2s2i?2  +  aigci  +  a2gci2 

2/i6  =  (2aiC2  +  2a2)t/i  +  (aic2  +  2a2)f/2  —  2aiS2‘di^2  -  a1s2ij% 
+gci2 
yn  =  ^1  +  ^2 
Vis  =  kr2&2 

2/21  =  0 
1/22  =  0 
1/23  =  0 
1/24  =  0 

1/25  =  (aia2c2  +  a2)^i  +  a|t?2  +  aia2S2i?i  +  a2gci2 
1/26  =  (aic2  +  2a2)i?i  +  2a2f/2  +  a\ s2t/2  +  gc\2 
1/27  =  t?i  +  t?2 
1/28  =  *V-2$1  +  fcr2^2- 


Example  7.2 

In  order  to  understand  the  relative  weight  of  the  various  torque  contributions  in  the 
dynamic  model  (7.82),  consider  a  two-link  planar  arm  with  the  following  data: 

ai  =  02  =  1  m  =  £2  =  0.5  m  me1  =  tn<2  =  50  kg  I(t  =  Ie2  =  10kg-m2 

kri  =  kr  2  =  100  mmi  =  m,m2  =  5  kg  Imi  =  Im2  =  0.01  kg-m2. 

The  two  links  have  been  chosen  equal  to  illustrate  better  the  dynamic  interaction 
between  the  two  joints. 

Figure  7.5  shows  the  time  history  of  positions,  velocities,  accelerations  and 
torques  resulting  from  joint  trajectories  with  typical  triangular  velocity  profile  and 
equal  time  duration.  The  initial  arm  configuration  is  so  that  the  tip  is  located  at  the 
point  (0.2,  0)  m  with  a  lower  elbow  posture.  Both  joints  make  a  rotation  of  7r/2rad 
in  a  time  of  0.5  s. 

From  the  time  history  of  the  single  torque  contributions  in  Fig.  7.6  it  can  be 
recognized  that: 

•  The  inertia  torque  at  Joint  1  due  to  Joint  1  acceleration  follows  the  time  history 
of  the  acceleration. 

•  The  inertia  torque  at  Joint  2  due  to  Joint  2  acceleration  is  piecewise  constant, 
since  the  inertia  moment  at  Joint  2  axis  is  constant. 


[Nm]  [Nm]  [Nm] 
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Fig.  7.8.  Time  history  of  torque  contributions  with  joint  trajectories  of  different 
duration 
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Fig.  7.9.  Time  history  of  tip  position,  velocity  and  acceleration  with  a  straight  line 
tip  trajectory  along  the  horizontal  axis 


•  The  inertia  torques  at  each  joint  due  to  acceleration  of  the  other  joint  confirm 
the  symmetry  of  the  inertia  matrix,  since  the  acceleration  profiles  are  the  same 
for  both  joints. 

•  The  Coriolis  effect  is  present  only  at  Joint  1,  since  the  arm  tip  moves  with  respect 
to  the  mobile  frame  attached  to  Link  1  but  is  fixed  with  respect  to  the  frame 
attached  to  Link  2. 

•  The  centrifugal  and  Coriolis  torques  reflect  the  above  symmetry. 

Figure  7.7  shows  the  time  history  of  positions,  velocities,  accelerations  and 
torques  resulting  from  joint  trajectories  with  typical  trapezoidal  velocity  profile  and 
different  time  duration.  The  initial  configuration  is  the  same  as  in  the  previous  case. 
The  two  joints  make  a  rotation  so  as  to  take  the  tip  to  the  point  (1.8, 0)  m.  The 
acceleration  time  is  0.15  s  and  the  maximum  velocity  is  5  rad/s  for  both  joints. 
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joint  1  pos  joint  2  pos 


Fig.  7.10.  Time  history  of  joint  positions,  velocities,  accelerations,  and  torques  with 
a  straight  line  tip  trajectory  along  the  horizontal  axis 
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From  the  time  history  of  the  single  torque  contributions  in  Fig.  7.8  it  can  be 
recognized  that: 

•  The  inertia  torque  at  Joint  1  due  to  Joint  2  acceleration  is  opposite  to  that  at 
Joint  2  due  to  Joint  1  acceleration  in  that  portion  of  trajectory  when  the  two 
accelerations  have  the  same  magnitude  but  opposite  sign. 

•  The  different  velocity  profiles  imply  that  the  centrifugal  effect  induced  at  Joint 
1  by  Joint  2  velocity  dies  out  later  than  the  centrifugal  effect  induced  at  Joint  2 
by  Joint  1  velocity. 

•  The  gravitational  torque  at  Joint  2  is  practically  constant  in  the  first  portion 
of  the  trajectory,  since  Link  2  is  almost  kept  in  the  same  posture.  As  for  the 
gravitational  torque  at  Joint  1,  instead,  the  centre  of  mass  of  the  articulated 
system  moves  away  from  the  origin  of  the  axes. 

Finally,  Fig.  7.9  shows  the  time  history  of  tip  position,  velocity  and  acceleration 
for  a  trajectory  with  a  trapezoidal  velocity  profile.  Starting  from  the  same  initial 
posture  as  above,  the  arm  tip  makes  a  translation  of  1.6  m  along  the  horizontal  axis; 
the  acceleration  time  is  0.15  s  and  the  maximum  velocity  is  5  m/s. 

As  a  result  of  an  inverse  kinematics  procedure,  the  time  history  of  joint  positions, 
velocities  and  accelerations  have  been  computed  which  are  illustrated  in  Fig.  7.10, 
together  with  the  joint  torques  that  are  needed  to  execute  the  assigned  trajectory. 
It  can  be  noticed  that  the  time  history  of  the  represented  quantities  differs  from 
the  corresponding  ones  in  the  operational  space,  in  view  of  the  nonlinear  effects 
introduced  by  kinematic  relations. 

For  what  concerns  the  time  history  of  the  individual  torque  contributions  in 
Fig.  7.11,  it  is  possible  to  make  a  number  of  remarks  similar  to  those  made  above 
for  trajectories  assigned  directly  in  the  joint  space. 


7.3.3  Parallelogram  Arm 


Consider  the  parallelogram  arm  in  Fig.  7.12.  Because  of  the  presence  of  the 
closed  chain,  the  equivalent  tree-structured  open-chain  arm  is  initially  taken 
into  account.  Let  ty,  iy  and  ty  be  the  distances  of  the  centres  of  mass 
of  the  three  links  along  one  branch  of  the  tree,  and  of  the  single  link  along 
the  other  branch,  from  the  respective  joint  axes.  Also  let  ,  mc2, ,  me3,  and 
me  „  be  the  masses  of  the  respective  links,  and  /q, ,  l/2,,  Ii3,  and  1^,,  the 
moments  of  inertia  relative  to  the  centres  of  mass  of  the  respective  links.  For 
the  sake  of  simplicity,  the  contributions  of  the  motors  are  neglected. 

With  the  chosen  coordinate  frames,  computation  of  the  Jacobians  in  (7.16) 
(7.18)  yields 


— £y  Sy  0  0 

S-H 

to 

II 
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— (,2'Sl'2'  0 

£yCy  0  0 

ai'Ci'  +  ^2'Cl'2' 
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whereas  computation  of  the  Jacobians  in  (7.17),  (7.19)  yields 
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From  (7.32),  the  inertia  matrix  of  the  virtual  arm  composed  of  joints  i?i/, 

$2',  $3'  is 


B'W) 


&l'l'(^2'i  ^3')  &1'2'(^2')  ^3')  &l'3'(^2'i  ^3') 

&2'l'(^2'i  ^3')  &2'2' ($3')  &2'3' (^3') 

&3'1'($2'>$3')  ^3'2'(^3')  &3'3' 


6i'i'  —  7^,  +ini1,£y  +  Ie2,  +  ro<2;  (flr  +^|'  +  2ai'^2'C2')  +  Ii3, 

■f^3,  (  ft-j  /  +  ft^/  “t“  ^3/  2fti/ft2/C2/  ~\~  2fti/^3/C2/3/  ~\~  2ft2'^3'C3') 

bi'2'  =  b'2'if  =  7^2,  +  m^2,  (tf/  +  ai't?2'C2')  +  h3, 

Jr'Wl(,3i  (®2'  d-  ^3'  “I”  ftl'ft2'C2'  +  ftl'^3' C2'3'  +  2ft2' £3' C3') 
frl'3'  =  ^31  =  If3,  +  nM  3/(^3'  +  «1,^3,C2'3'  +  a2'^3'C3') 
b‘2'2'  =  It2 /  +  2'  “1“  7^3 ,  +  m^3,  (a|/  +  $3/  +  2a2'£3'C3') 

^2'3'  =  h3 /  +  m^3/  (^3'  +  ft2'73'C3') 

^3'3'  =  7^3,  +  m^3,  £3/ 
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while  the  moment  of  inertia  of  the  virtual  arm  composed  of  just  joint  'dy  is 

&i"i"  =  h-,,,  + 

Therefore,  the  inertial  torque  contributions  of  the  two  virtual  arms  are  re¬ 
spectively: 

3' 

Tp  =  T l"  =  6p/p/$p/. 

J'= 1' 

At  this  point,  in  view  of  (2.64)  and  (3.121),  the  inertial  torque  contribu¬ 
tions  at  the  actuated  joints  for  the  closed-chain  arm  turn  out  to  be 

Ta  =  Baqa 

where  qa  =  [i?i»  dy  ]T,  ra  =  [rai  ra2  )T  and 

r,  _  ball  ba  12 

a  —  7  j 

0a21  0a  22 

bail  =  7^/  +  me1,£i,  +  rri(,2,a\f  +  J^3,  +  mt3,£ +  m£3,a\/ 
-2avmi3,t-i' 

ball  =  fra21  =  (ai- 7714,^2'  +  (dp  -  £3'))  COS  (dy  -  $1') 

&a22  =  h±,  +  +  /e2,  +  m<2,^2'  +  mt3,a\„. 

This  expression  reveals  the  possibility  of  obtaining  a  configuration-independent 
and  decoupled  inertia  matrix;  to  this  end  it  is  sufficient  to  design  the  four  links 
of  the  parallelogram  so  that 

me3,£3 <  _  ap 
me2,£2'  ay 

where  £3 /  =  £3'  —  ap  is  the  distance  of  the  centre  of  mass  of  Link  3'  from  the 
axis  of  Joint  4.  If  this  condition  is  satisfied,  then  the  inertia  matrix  is  diagonal 

{ball  =  ba21  =  0)  with 


bail  —  Ielf  +  rme1,£y  +  tni  ,a±i  ( 1  -I — - — - — j  +  Ie  , 

V  avay  J 

ba 22  =  7^,  +  me^iy  +  I(,2,  +  me2,£ %  ^1  +  ^  ^  . 


As  a  consequence,  no  contributions  of  Coriolis  and  centrifugal  torques  are 
obtained.  Such  a  result  could  not  be  achieved  with  the  previous  two-link 
planar  arm,  no  matter  how  the  design  parameters  were  chosen. 
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As  for  the  gravitational  terms,  since  g0  =  [0  —g  0]T,  (7.39)  with  the 
above  Jacobians  gives 


9v  =  (me^i  v  +  rnz2,av  +  mt3,av)gcv  +  (m^ty  +  mta,a2')gcvv 

+'me3,£3,gci'2,3 

g2,  =  +  mi3la2')gcV2'  +  me3,iygcV2>3 

53'  =  rne3,£3'gci'2'3 


and 


g  i"  =  mi^iy/gcy- 


Composing  the  various  contributions  as  done  above  yields 


9a 


(m^iv  +  mt2,av  -  mia,tz')gcv 
+  me2,t  2>  +  mia,av>)gcy 


which,  together  with  the  inertial  torques,  completes  the  derivation  of  the 
sought  dynamic  model. 

A  final  comment  is  in  order.  In  spite  of  its  kinematic  equivalence  with  the 
two-link  planar  arm,  the  dynamic  model  of  the  parallelogram  is  remarkably 
lighter.  This  property  is  quite  advantageous  for  trajectory  planning  and  con¬ 
trol  purposes.  For  this  reason,  apart  from  obvious  considerations  related  to 
manipulation  of  heavy  payloads,  the  adoption  of  closed  kinematic  chains  in 
the  design  of  industrial  robots  has  received  a  great  deal  of  attention. 


7.4  Dynamic  Parameter  Identification 

The  use  of  the  dynamic  model  for  solving  simulation  and  control  problems  de¬ 
mands  the  knowledge  of  the  values  of  dynamic  parameters  of  the  manipulator 
model. 

Computing  such  parameters  from  the  design  data  of  the  mechanical  struc¬ 
ture  is  not  simple.  CAD  modelling  techniques  can  be  adopted  which  allow  the 
computation  of  the  values  of  the  inertial  parameters  of  the  various  components 
(links,  actuators  and  transmissions)  on  the  basis  of  their  geometry  and  type  of 
materials  employed.  Nevertheless,  the  estimates  obtained  by  such  techniques 
are  inaccurate  because  of  the  simplification  typically  introduced  by  geometric 
modelling;  moreover,  complex  dynamic  effects,  such  as  joint  friction,  cannot 
be  taken  into  account. 

A  heuristic  approach  could  be  to  dismantle  the  various  components  of  the 
manipulator  and  perform  a  series  of  measurements  to  evaluate  the  inertial 
parameters.  Such  technique  is  not  easy  to  implement  and  may  be  troublesome 
to  measure  the  relevant  quantities. 

In  order  to  find  accurate  estimates  of  dynamic  parameters,  it  is  worth 
resorting  to  identification  techniques  which  conveniently  exploit  the  property 
of  linearity  (7.81)  of  the  manipulator  model  with  respect  to  a  suitable  set  of 
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dynamic  parameters.  Such  techniques  allow  the  computation  of  the  parameter 
vector  7r  from  the  measurements  of  joint  torques  t  and  of  relevant  quantities 
for  the  evaluation  of  the  matrix  Y,  when  suitable  motion  trajectories  are 
imposed  to  the  manipulator. 

On  the  assumption  that  the  kinematic  parameters  in  the  matrix  Y  are 
known  with  good  accuracy,  e.g.,  as  a  result  of  a  kinematic  calibration,  mea¬ 
surements  of  joint  positions  q,  velocities  q  and  accelerations  q  are  required. 
Joint  positions  and  velocities  can  be  actually  measured  while  numerical  recon¬ 
struction  of  accelerations  is  needed;  this  can  be  performed  on  the  basis  of  the 
position  and  velocity  values  recorded  during  the  execution  of  the  trajectories. 
The  reconstructing  filter  does  not  work  in  real  time  and  thus  it  can  also  be 
anti-causal,  allowing  an  accurate  reconstruction  of  the  accelerations. 

As  regards  joint  torques,  in  the  unusual  case  of  torque  sensors  at  the 
joint,  these  can  be  measured  directly.  Otherwise,  they  can  be  evaluated  from 
either  wrist  force  measurements  or  current  measurements  in  the  case  of  electric 
actuators. 

If  measurements  of  joint  torques,  positions,  velocities  and  accelerations 
have  been  obtained  at  given  time  instants  along  a  given  trajectory, 

one  may  write 


T  = 


T(tl) 

'  Y(t i)  ' 

(7.85) 


The  number  of  time  instants  sets  the  number  of  measurements  to  perform 
and  should  be  large  enough  (typically  Nn  p)  so  as  to  avoid  ill-conditioning 
of  matrix  Y .  Solving  (7.85)  by  a  least-squares  technique  leads  to  the  solution 
in  the  form 


7T  =  (YTY)~1YTT 


(7.86) 


where  ( YTY)~1YT  is  the  left  pseudo-inverse  matrix  of  Y. 

It  should  be  noticed  that,  in  view  of  the  block  triangular  structure  of 
matrix  Y  in  (7.80),  computation  of  parameter  estimates  could  be  simplified 
by  resorting  to  a  sequential  procedure.  Take  the  equation  rn  =  y^n 7r„  and 
solve  it  for  nn  by  specifying  r„  and  y^n  for  a  given  trajectory  on  Joint  n. 
By  iterating  the  procedure,  the  manipulator  parameters  can  be  identified  on 
the  basis  of  measurements  performed  joint  by  joint  from  the  outer  link  to  the 
base.  Such  procedure,  however,  may  have  the  inconvenience  to  accumulate 
any  error  due  to  ill-conditioning  of  the  matrices  involved  step  by  step.  It  may 
then  be  worth  operating  with  a  global  procedure  by  imposing  motions  on  all 
manipulator  joints  at  the  same  time. 

Regarding  the  rank  of  matrix  Y ,  it  is  possible  to  identify  only  the  dynamic 
parameters  of  the  manipulator  that  contribute  to  the  dynamic  model.  Exam¬ 
ple  7.2  has  indeed  shown  that  for  the  two-link  planar  arm  considered,  only 
8  out  of  the  22  possible  dynamic  parameters  appear  in  the  dynamic  model. 
Hence,  there  exist  some  dynamic  parameters  which,  in  view  of  the  disposition 
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of  manipulator  links  and  joints,  are  non-identifiable,  since  for  any  trajectory 
assigned  to  the  structure  they  do  not  contribute  to  the  equations  of  motion.  A 
direct  consequence  is  that  the  columns  of  the  matrix  Y  in  (7.80)  correspond¬ 
ing  to  such  parameters  are  null  and  thus  they  have  to  be  removed  from  the 
matrix  itself;  e.g.,  the  resulting  (2  x  8)  matrix  in  (7.84). 

Another  issue  to  consider  about  determination  of  the  effective  number 
of  parameters  that  can  be  identified  by  (7.86)  is  that  some  parameters  can 
be  identified  in  linear  combinations  whenever  they  do  not  appear  isolated  in 
the  equations.  In  such  a  case,  it  is  necessary,  for  each  linear  combination,  to 
remove  as  many  columns  of  the  matrix  Y  as  the  number  of  parameters  in  the 
linear  combination  minus  one. 

For  the  determination  of  the  minimum  number  of  identifiable  parameters 
that  allow  direct  application  of  the  least-squares  technique  based  on  (7.86), 
it  is  possible  to  inspect  directly  the  equations  of  the  dynamic  model,  as  long 
as  the  manipulator  has  few  joints.  Otherwise,  numerical  techniques  based  on 
singular  value  decomposition  of  matrix  Y  have  to  be  used.  If  the  matrix  Y 
resulting  from  a  series  of  measurements  is  not  full-rank,  one  has  to  resort  to 
a  damped  least-squares  inverse  of  Y  where  solution  accuracy  depends  on  the 
weight  of  the  damping  factor. 

In  the  above  discussion,  the  type  of  trajectory  imposed  to  the  manipulator 
joints  has  not  been  explicitly  addressed.  It  can  be  generally  ascertained  that 
the  choice  should  be  oriented  in  favor  of  polynomial  type  trajectories  which  are 
sufficiently  rich  to  allow  an  accurate  evaluation  of  the  identifiable  parameters. 

~  T  - 

This  corresponds  to  achieving  a  low  condition  number  of  the  matrix  Y  Y 
along  the  trajectory.  On  the  other  hand,  such  trajectories  should  not  excite 
any  unmodelled  dynamic  effects  such  as  joint  elasticity  or  link  flexibility  that 
would  naturally  lead  to  unreliable  estimates  of  the  dynamic  parameters  to 
identify. 

Finally,  it  is  worth  observing  that  the  technique  presented  above  can  also 
be  extended  to  the  identification  of  the  dynamic  parameters  of  an  unknown 
payload  at  the  manipulator’s  end-effector.  In  such  a  case,  the  payload  can  be 
regarded  as  a  structural  modification  of  the  last  link  and  one  may  proceed  to 
identify  the  dynamic  parameters  of  the  modified  link.  To  this  end,  if  a  force 
sensor  is  available  at  the  manipulator’s  wrist,  it  is  possible  to  characterize 
directly  the  dynamic  parameters  of  the  payload  starting  from  force  sensor 
me  asurement  s . 


7.5  Newton  Euler  Formulation 

In  the  Lagrange  formulation,  the  manipulator  dynamic  model  is  derived  start¬ 
ing  from  the  total  Lagrangian  of  the  system.  On  the  other  hand,  the  Newton- 
Euler  formulation  is  based  on  a  balance  of  all  the  forces  acting  on  the  generic 
link  of  the  manipulator.  This  leads  to  a  set  of  equations  whose  structure  allows 
a  recursive  type  of  solution;  a  forward  recursion  is  performed  for  propagating 
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/ 


Fig.  7.13.  Characterization  of  Link  i  for  Newton-Euler  formulation 


link  velocities  and  accelerations,  followed  by  a  backward  recursion  for  propa¬ 
gating  forces. 

Consider  the  generic  augmented  Link  i  (Link  i  plus  motor  of  Joint  ?'  +  l)  of 
the  manipulator  kinematic  chain  (Fig.  7.13).  According  to  what  was  presented 
in  Sect.  7.2.2,  one  can  refer  to  the  centre  of  mass  C,  of  the  augmented  link  to 
characterize  the  following  parameters: 

•  TOi  mass  of  augmented  link, 

•  Ii  inertia  tensor  of  augmented  link, 

•  Imi  moment  of  inertia  of  rotor, 

•  vector  from  origin  of  Frame  (i  —  1)  to  centre  of  mass  C), 

•  ri,Ci  vector  from  origin  of  Frame  i  to  centre  of  mass  C), 

•  vector  from  origin  of  Frame  ( i  —  1)  to  origin  of  Frame  i. 

The  velocities  and  accelerations  to  be  considered  are: 

•  pc  linear  velocity  of  centre  of  mass  C), 

•  p,  linear  velocity  of  origin  of  Frame  i, 

•  angular  velocity  of  link, 

•  ujm.  angular  velocity  of  rotor, 

•  pc  linear  acceleration  of  centre  of  mass  Cj, 

•  p,  linear  acceleration  of  origin  of  Frame  i, 

•  uji  angular  acceleration  of  link, 

•  u>mi  angular  acceleration  of  rotor, 

•  g 0  gravity  acceleration. 

The  forces  and  moments  to  be  considered  are: 

•  force  exerted  by  Link  i-lon  Link  i, 

•  —fi+i  force  exerted  by  Link  i  +  1  on  Link  i, 
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•  /xi  moment  exerted  by  Link  i  —  1  on  Link  i  with  respect  to  origin  of 
Frame  i  —  1, 

•  —  ni+i  moment  exerted  by  Link  i  +  1  on  Link  i.  with  respect  to  origin  of 
Frame  i. 


Initially,  all  the  vectors  and  matrices  are  assumed  to  be  expressed  with 
reference  to  the  base  frame. 

As  already  anticipated,  the  Newton-Euler  formulation  describes  the  mo¬ 
tion  of  the  link  in  terms  of  a  balance  of  forces  and  moments  acting  on  it. 

The  Newton  equation  for  the  translational  motion  of  the  centre  of  mass 
can  be  written  as 

fi  -  fi+ 1  +  m,90  =  mtpCi.  (7.87) 

The  Euler  equation  for  the  rotational  motion  of  the  link  (referring  mo¬ 
ments  to  the  centre  of  mass)  can  be  written  as 


Ab  T  fi  x  Ti  —  i(Ji  |_1  f  i+l  X  Ti^Ci  ^  (li^i  +  kr,i+lQi+lImi+1  zmi+1), 


(7.88) 

where  (7.67)  has  been  used  for  the  angular  momentum  of  the  rotor.  Notice 
that  the  gravitational  force  rriig 0  does  not  generate  any  moment,  since  it  is 
concentrated  at  the  centre  of  mass. 

As  pointed  out  in  the  above  Lagrange  formulation,  it  is  convenient  to 
express  the  inertia  tensor  in  the  current  frame  (constant  tensor).  Hence,  ac¬ 
cording  to  (7.12),  one  has  Jj  =  Ril^Rf,  where  F£,  is  the  rotation  matrix  from 
Frame  i  to  the  base  frame.  Substituting  this  relation  in  the  first  term  on  the 
right-hand  side  of  (7.88)  yields 


4(j<«i)  =  +  Ril\R^u)i  (7.89) 

at 

=  Siu^RifiRjoOi  +  RjiiRTST{u>i)ui  +  R,I,R‘^, 

=  IiU>i  +  uj.i  x  (hufi) 


where  the  second  term  represents  the  gyroscopic  torque  induced  by  the  depen¬ 
dence  of  I,  on  link  orientation.7  Moreover,  by  observing  that  the  unit  vector 
zmj+i  rotates  accordingly  to  Link  i,  the  derivative  needed  in  the  second  term 
on  the  right-hand  side  of  (7.88)  is 

^  ^mj+i  4“  X  Zrru+i  (7.90) 

By  substituting  (7.89),  (7.90)  in  (7.88),  the  resulting  Euler  equation  is 
Ab  +  fi  x  — Ab+i  ~  fi+i  x  ri,Ci  =  liUi  +  w,  x  (/jW,)  (7.91) 

-\-kr}i+iqi+lImi+1Zmi+i  T  kr,i+l  qiJr\Irrli+1  LO i  X  Zrrli+1. 

1  In  deriving  (7.89),  the  operator  S  has  been  introduced  to  compute  the  derivative 
of  Ri,  as  in  (3.8);  also,  the  property  ST(u>i)u}i  =  0  has  been  utilized. 
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The  generalized  force  at  Joint  i  can  be  computed  by  projecting  the  force 
f  i  for  a  prismatic  joint,  or  the  moment  for  a  revolute  joint,  along  the 
joint  axis.  In  addition,  there  is  the  contribution  of  the  rotor  inertia  torque 
krilmi^'m- zmi.  Hence,  the  generalized  force  at  Joint  i  is  expressed  by 


fi  Zi—  1  T  krilrrii^ 


A"rn i 


fJ'i  Z>i— 1  H-  krilrrii^ 


•T 


for  a  prismatic  joint 
for  a  revolute  joint. 


(7.92) 


7.5.1  Link  Accelerations 


The  Newton-Euler  equations  in  (7.87),  (7.91)  and  the  equation  in  (7.92)  re¬ 
quire  the  computation  of  linear  and  angular  acceleration  of  Link  i  and  Rotor 
i.  This  computation  can  be  carried  out  on  the  basis  of  the  relations  expressing 
the  linear  and  angular  velocities  previously  derived.  The  equations  in  (3.21), 
(3.22),  (3.25),  (3.26)  can  be  briefly  rewritten  as 


and 


f  u;,-!  for  a  prismatic  joint 

(  u>i- i  +  ttiZi- 1  for  a  revolute  joint 


(7.93) 


Pi  = 


Pi-i  +  diZi- 1  +  u>i  x 
Pi-!  +  oj.i  x  r i—i  i 


for  a  prismatic  joint  ^ 
for  a  revolute  joint. 


As  for  the  angular  acceleration  of  the  link,  it  can  be  seen  that,  for  a 
prismatic  joint,  differentiating  (3.21)  with  respect  to  time  gives 


—  d>i-i, 


(7.95) 


whereas,  for  a  revolute  joint,  differentiating  (3.25)  with  respect  to  time  gives 
uJi  =  u>i- 1  +  diZi- 1  +  x  Zi- 1.  (7.96) 


As  for  the  linear  acceleration  of  the  link,  for  a  prismatic  joint,  differenti¬ 
ating  (3.22)  with  respect  to  time  gives 

Pi  =  Pi-i  +  d^i- 1  +  diU>i- 1  x  Zi- 1  +  uJi  x  (7.97) 

+uji  x  diZi- 1  +  u,x  (u>i_ i  x  ri_i,j) 

where  the  relation  =  diZi-i  +  0Ji-i  x  rq_iy  has  been  used.  Hence,  in 

view  of  (3.21),  the  equation  in  (7.97)  can  be  rewritten  as 

Pi  =  Pi- 1  +  d^i- 1  +  2djWi  x  Zj_i  +  x  +  w,  x  (w*  x  r,_i)j;).  (7.98) 


Also,  for  a  revolute  joint,  differentiating  (3.26)  with  respect  to  time  gives 

Pi  =  Pi-i  +  dJiX  rj;_M  +  LOi  x  (wj  x  r i—i  i} .  (7.99) 
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In  summary,  the  equations  in  (7.95),  (7.96),  (7.98),  (7.99)  can  be  compactly 
rewritten  as 


d>i- 1 

Ui-l  +  +  fliUJi- 1  x  Zj_i 


for  a  prismatic  joint 
for  a  revolute  joint 


(7.100) 


and 


{Pi-i  +  <kzi- 1  +  2djWj  x  Zj_i 
+0;*  x  +  u>i  x  (wj  x  ri_i,j) 

Pi-!  +Vi  x  ri_M 
+Wi  x  (u>i  x  ri_i,i) 


for  a  prismatic  joint 

(7.101) 

for  a  revolute  joint. 


The  acceleration  of  the  centre  of  mass  of  Link  i  required  by  the  Newton 
equation  in  (7.87)  can  be  derived  from  (3.15),  since  r\c.  =  0;  by  differenti¬ 
ating  (3.15)  with  respect  to  time,  the  acceleration  of  the  centre  of  mass  Ct 
can  be  expressed  as  a  function  of  the  velocity  and  acceleration  of  the  origin 
of  Frame  i,  i.e., 


PCi  =  Pi  +  6>i  X  ntCi  +  x  (w»  x  ritc J-  (7.102) 

Finally,  the  angular  acceleration  of  the  rotor  can  be  obtained  by  time 
differentiation  of  (7.23),  i.e., 

mi  =  02 i—  1  T  kri(jjZmi  T  kriQi^i—  1  X  Zrni.  (7.103) 

7.5.2  Recursive  Algorithm 

It  is  worth  remarking  that  the  resulting  Newton-Euler  equations  of  motion 
are  not  in  closed  form,  since  the  motion  of  a  single  link  is  coupled  to  the 
motion  of  the  other  links  through  the  kinematic  relationship  for  velocities 
and  accelerations. 

Once  the  joint  positions,  velocities  and  accelerations  are  known,  one  can 
compute  the  link  velocities  and  accelerations,  and  the  Newton-Euler  equations 
can  be  utilized  to  find  the  forces  and  moments  acting  on  each  link  in  a  recur¬ 
sive  fashion,  starting  from  the  force  and  moment  applied  to  the  end-effector. 
On  the  other  hand,  also  link  and  rotor  velocities  and  accelerations  can  be 
computed  recursively  starting  from  the  velocity  and  acceleration  of  the  base 
link.  In  summary,  a  computationally  recursive  algorithm  can  be  constructed 
that  features  a  forward  recursion  relative  to  the  propagation  of  velocities  and 
accelerations  and  a  backward  recursion  for  the  propagation  of  forces  and  mo¬ 
ments  along  the  structure. 

For  the  forward  recursion,  once  q ,  q,  q,  and  the  velocity  and  acceleration 
of  the  base  link  wq,  i>o  —  9o>  are  specified,  Wj,  u>i,  Pj,  Pc0  can  be 
computed  using  (7.93),  (7.100),  (7.101),  (7.102),  (7.103),  respectively.  Notice 
that  the  linear  acceleration  has  been  taken  as  p0  —  g0  so  as  to  incorporate  the 
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term  —  gQ  in  the  computation  of  the  acceleration  of  the  centre  of  mass  pc. 
via  (7.101),  (7.102). 

Having  computed  the  velocities  and  accelerations  with  the  forward  recur¬ 
sion  from  the  base  link  to  the  end-effector,  a  backward  recursion  can  be  carried 
out  for  the  forces.  In  detail,  once  he  =  [f^+i  /■*«+ i]T  is  given  (eventually 

he  =  0),  the  Newton  equation  in  (7.87)  to  be  used  for  the  recursion  can  be 
rewritten  as 

Si  =  fi+i  +  mpCi  (7.104) 

since  the  contribution  of  gravity  acceleration  has  already  been  included  in 
Pc  ■  Further,  the  Euler  equation  gives 

Mz  =  ~fi  x  (n-M  +  ritd)  +  Mi+i  +  /<+ 1  x  rifCi  +  x  (1^) 

mi_(_  i  (7.105) 


which  derives  from  (7.91),  where  ry-ycy  has  been  expressed  as  the  sum  of  the 
two  vectors  appearing  already  in  the  forward  recursion.  Finally,  the  general¬ 
ized  forces  resulting  at  the  joints  can  be  computed  from  (7.92)  as 


Si  1  4“  kriSrrii 

+FVidi  +  FSi  sgn  ( di )  for  a  prismatic  joint 

Pi  1  4“  mi^mi 

+FVidi+  FSi  sgn  (di)  for  a  revolute  joint, 


(7.106) 


where  joint  viscous  and  Coulomb  friction  torques  have  been  included. 

In  the  above  derivation,  it  has  been  assumed  that  all  vectors  were  referred 
to  the  base  frame.  To  simplify  greatly  computation,  however,  the  recursion  is 
computationally  more  efficient  if  all  vectors  are  referred  to  the  current  frame 
on  Link  i.  This  implies  that  all  vectors  that  need  to  be  transformed  from 
Frame  i  +  1  into  Frame  i  have  to  be  multiplied  by  the  rotation  matrix  R],  l5 
whereas  all  vectors  that  need  to  be  transformed  from  Frame  i  —  1  into  Frame  i 
have  to  be  multiplied  by  the  rotation  matrix  R~lT .  Therefore,  the  equations 
in  (7.93),  (7.100),  (7.101),  (7.102),  (7.103),  (7.104),  (7.105),  (7.106)  can  be 
rewritten  as: 


1  for  a  prismatic  joint 

~~ lT(u>lzl  +  diZo)  for  a  revolute  joint 


(7.107) 


wi-l 


for  a  prismatic  joint 


1  1t(lo\_\  +  diZ0  +  diUj\_\  x  z 0)  for  a  revolute  joint 
+  +  2 d,uj\  x  FC~1t Zq 


(7.108) 


Pi=  < 


4 -^i  x  r\_ hi  +  u>\  x  (col  x 

RtlTiCl+^xrt_iti 

{  +wj  x  (uj\  x  r\_hi) 


for  a  prismatic  joint 

(7.109) 


for  a  revolute  joint 
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Fig.  7.14.  Computational  structure  of  the  Newton-Euler  recursive  algorithm 


P'Ci  =Pi  +  ^x  rii  Ci  +  x  («j  x  rj)CJ 

=  ^i—l  T  k riQiZm,i  d"  —  \  X  Zmf 

ft  =  Ri+ift$l+mipbi 


K  —  —fi  x  (ri-l,i  +  ri,Ci)  +  ^i+lMi+1  +  -Ri+l/i+1  x  ri,Ci 


+1,^1  +  U l  x  (1,^1) 

+  fcr,i+l9i+l^mi+i2:rni+ 


1  T 


U)  a  X  z! 


(7.110) 

(7.111) 

(7.112) 

(7.113) 


(f^BtlTz0  +  kriIm^z^ 
\  +Fvidi  +  Fsi  sgn  (dj) 

I  niT  IT  ~  I  Z-  T 
I  Mi  -“'i  ^0  ~i  K,ri±mi^rni  Zm,i 

{  +Fvii}i  +  Fsj  sgn  (dj) 


for  a  prismatic  joint 

(7.114) 

for  a  revolute  joint. 


The  above  equations  have  the  advantage  that  the  quantities  l\,  r^c. ,  z^.1 
are  constant,  further,  it  is  Zq  =  [0  0  1]T. 

To  summarize,  for  given  joint  positions,  velocities  and  accelerations,  the 
recursive  algorithm  is  carried  out  in  the  following  two  phases: 
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•  With  known  initial  conditions  Wq,  jig  —  <7°,  and  chg,  use  (7.107),  (7.108), 
(7.109),  (7.110),  (7.111),  for  i  =  1, . . .  ,n,  to  compute  wf,  u],  p\,  Pc.,u}l~^. 

•  With  known  terminal  conditions  f^+i  and  i>  use  (7.112),  (7.113),  for 
i  =  n, . . . ,  1,  to  compute  f\,  p\,  and  then  (7.114)  to  compute  r*. 

The  computational  structure  of  the  algorithm  is  schematically  illustrated 
in  Fig.  7.14. 


7.5.3  Example 

In  the  following,  an  example  to  illustrate  the  single  steps  of  the  Newton- 
Euler  algorithm  is  developed.  Consider  the  two- link  planar  arm  whose  dy¬ 
namic  model  has  already  been  derived  in  Example  7.2. 

Start  by  imposing  the  initial  conditions  for  the  velocities  and  accelerations: 

Po-9'o=[0  9  Of  wg=d;g  =  0, 

and  the  terminal  conditions  for  the  forces: 

fl  =  0  Ms  =  0. 

All  quantities  are  referred  to  the  current  link  frame.  As  a  consequence,  the 
following  constant  vectors  are  obtained: 


r1 

rl,Ci  - 

A/ 

0 

A  = 

i 

,  ®  ° 

r-2 

r2 ,C2  - 

£c2 

0 

r2  - 
*1,2  ~ 

<§■  o 

1 _ 

0 

0 

0 

o 

where  icx  and  are  both  negative  quantities.  The  rotation  matrices  needed 
for  vector  transformation  from  one  frame  to  another  are 


R 


,i-l 


Cj  Si  0 
Si  Ci  0 

0  0  1 


*=1,2  Rl  =  I. 


Further,  it  is  assumed  that  the  axes  of  rotation  of  the  two  rotors  coincide  with 
the  respective  joint  axes,  i.e. ,  z =  z0  =  [  0  0  1  ]T  for  i  =  1,  2. 

According  to  (7.107)-(7.114),  the  Newton-Euler  algorithm  requires  the 
execution  of  the  following  steps: 

•  Forward  recursion:  Link  1 


u> 


l 

l 


'  0  ' 
0 

A. 
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UJ 


l 

l 


"  0  “ 
0 

A. 


pi  = 


-ai'&l  +gs\ 
a-i'&i  +  gci 
0 


Pk 


-(^Ci  +  ai)i?i  +  gsi 
(£Cl  +  ai)i?i  +  5C1 
0 


'  0 

cJ°  =  0 

"mi 

•  Forward  recursion:  Link  2 


Cl! 


2 

2 


0 

0 

$1  +  l?2 


cL> 


2 

2 


0 

0 

+  1?2 


P2  = 


ais2$i  —  aiC2i?i  —  a2($i  +  $2)2  +  5Si2 
aic2^i  +  a2(i?i  +  $2)  +  ais2i?i  +  gc  12 


0 


Pc2 


a\S2$i  -  oic2i?i  -  ( lc2  +  a2)0 i  +  $2)2  +  5^12 
aic2i?i  +  ( ic2  +  o2)('i?i  +  $2)  +  aiS2'&l  +  gc\2 
0 


U! 


1 

m2 


0 

0 

$1  +  kr  2$2 
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•  Backward  recursion:  Link  2 

'to2(oi52'!?1  -  CHC2^1  -  ((-C2  +  +  $2)2  +  ffSl2) 

f  2  =  TO2(oic2t?i  +  (£c2  +  a2)(#i  +  #2)  +  ais2i?i  +  gci2) 

0 

* 

* 

-^2zz(Xl  +  ^2)  +  tn-2  (£c2  +  a2)2(^l  +  $2)  +  W2Ol(^C2  +  a2)c2$l 
+?n2ai(^c2  +  a2)s2t?f  +  m2(ic2  +  a2)gci2 

r2  =  (/2zz  +  m2  ((£c2  +  a2)2  +  ai(£c2  +  a2)c2)  +  kr2Im2)^i 

+  (I2  zz  +  W2fc  +  02) 2  +  kr2^m2  )  ^2 

+m2ai(£c2  +  a2)s2^2  +  m2(£c2  +  a2)gci2. 

•  Backward  recursion:  Link  1 

'  —m2(tc2  +  a2)s2(fli  +  $2)  —  +  oi)$i  —  m2ai,d\ 

—m2{tc2  +  a2)c2($i  +  t?2)2  +  (toi  +  m2)gs  1 
f  \  =  mi^Ct  +  ai)$i  +  m2a\,di  +  m2(£c2  +  0,2)02(1)1  +  i92) 

-m2(ic2  +  a2)s2(t?i  +  t^)2  +  (mi  +  m.2)gci 

0 

* 

* 

/izztfi  +  m2a2$i  +  mi(£c1  +  ai)2$i  +  m2ai(£c2  +  02)02^1 

Vi  =  +hzz(&\  +  ^2)  +  rn.2ai((.c2  +  02)02(^1  +  $2) 

+m2(^c2  +  a2)2(^i  +  i)2)  +  k  r2-^m2  ^2 
+m2ai(£c2  +  02X2$?  -  m2ai(£C2  +  a2)s2(i)  1  +  i)2)2 
+mi(£ci  +  ai)<?ci  +  m2aigci  +  m2(£c2  +  a2)gci2 

7~1  —  ^Ilzz  +  m,i(£c  1  +  ai)2  +  krilmi  H-  I2 zz 

+m2  (a2  +  (ic2  +  a2)2  +  2ai(£c2  +  a2)c2))i?i 

+  (^2zz  +  m2  ((^c2  +  a2)2  +  ai(^c2  +  02)02)  +  k  r2-^m2^  t)2 

— 2m2ai(^c2  +  02)s2’&i’&2  —  m2ai(£c2  +  a2)s2t)2 

+  (mi(£Cl  +  ai)  +  m.2ai)gci  +  m2(£c2  +  a2)gci2. 
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As  for  the  moment  components,  those  marked  by  the  symbol  V  have  not 
been  computed,  since  they  are  not  related  to  the  joint  torques  T2  and  t\ . 

Expressing  the  dynamic  parameters  in  the  above  torques  as  a  function  of 
the  link  and  rotor  parameters  as  in  (7.83)  yields 


mi  =  me1  +  mm2 
toi^Ci  =  mi i(fi  -  ai) 

hzz  +  mi4  =  Ii  =  I  lx  +  mi  1(^1 
m2  =  m^2 

TO2^c2  =  mi2(£2  -  02) 
+  m'2^c2  =  I2  =  h2  +  to^2  (^2 


Ol)2  +  /m2 


a2)2- 


On  the  basis  of  these  relations,  it  can  be  verified  that  the  resulting  dynamic 
model  coincides  with  the  model  derived  in  (7.82)  with  Lagrange  formulation. 


7.6  Direct  Dynamics  and  Inverse  Dynamics 

Both  Lagrange  formulation  and  Newton-Euler  formulation  allow  the  compu¬ 
tation  of  the  relationship  between  the  joint  torques  —  and,  if  present,  the 
end-effector  forces  —  and  the  motion  of  the  structure.  A  comparison  between 
the  two  approaches  reveals  what  follows.  The  Lagrange  formulation  has  the 
following  advantages: 

•  It  is  systematic  and  of  immediate  comprehension. 

•  It  provides  the  equations  of  motion  in  a  compact  analytical  form  containing 
the  inertia  matrix,  the  matrix  in  the  centrifugal  and  Coriolis  forces,  and 
the  vector  of  gravitational  forces.  Such  a  form  is  advantageous  for  control 
design. 

•  It  is  effective  if  it  is  wished  to  include  more  complex  mechanical  effects 
such  as  flexible  link  deformation. 

The  Newton-Euler  formulation  has  the  following  fundamental  advantage: 

•  It  is  an  inherently  recursive  method  that  is  computationally  efficient. 

In  the  study  of  dynamics,  it  is  relevant  to  find  a  solution  to  two  kinds  of 
problems  concerning  computation  of  direct  dynamics  and  inverse  dynamics. 

The  direct  dynamics  problem  consists  of  determining,  for  t  >  to,  the  joint 
accelerations  q(t)  (and  thus  q(t),  q{t))  resulting  from  the  given  joint  torques 
t(£)  —  and  the  possible  end-effector  forces  he(t)  —  once  the  initial  positions 
q(to)  and  velocities  q(to)  are  known  (initial  state  of  the  system). 

The  inverse  dynamics  problem  consists  of  determining  the  joint  torques 
r(f)  which  are  needed  to  generate  the  motion  specified  by  the  joint  accelera¬ 
tions  q(t),  velocities  q(t),  and  positions  q(t)  —  once  the  possible  end-effector 
forces  he(t)  are  known. 
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Solving  the  direct  dynamics  problem  is  useful  for  manipulator  simulation. 
Direct  dynamics  allows  the  motion  of  the  real  physical  system  to  be  described 
in  terms  of  the  joint  accelerations,  when  a  set  of  assigned  joint  torques  is 
applied  to  the  manipulator;  joint  velocities  and  positions  can  be  obtained  by 
integrating  the  system  of  nonlinear  differential  equations. 

Since  the  equations  of  motion  obtained  with  Lagrange  formulation  give  the 
analytical  relationship  between  the  joint  torques  (and  the  end-effector  forces) 
and  the  joint  positions,  velocities  and  accelerations,  these  can  be  computed 
from  (7.42)  as 

Q  =  B^1{q){r  -  t')  (7.115) 

where 

r'(q,  q)  =  C(q,  q)q  +  Fvq  +  Fs  sgn  (q)  +  g(q)  +  JT(q)he  (7.116) 

denotes  the  torque  contributions  depending  on  joint  positions  and  velocities. 
Therefore,  for  simulation  of  manipulator  motion,  once  the  state  at  the  time 
instant  tk  is  known  in  terms  of  the  position  q{tk)  and  velocity  q(tk),  the  accel¬ 
eration  q(tk)  can  be  computed  by  (7.115).  Then  using  a  numerical  integration 
method,  e.g.,  Runge-Kutta,  with  integration  step  At,  the  velocity  q{tk+ i)  and 
position  q(tk+i)  at  the  instant  tk+i  =  tk  +  At  can  be  computed. 

If  the  equations  of  motion  are  obtained  with  Newton-Euler  formulation, 
it  is  possible  to  compute  direct  dynamics  by  using  a  computationally  more 
efficient  method.  In  fact,  for  given  q  and  q,  the  torques  r'(q,  q)  in  (7.116)  can 
be  computed  as  the  torques  given  by  the  algorithm  of  Fig.  7.14  with  q  =  0. 
Further,  column  fo;  of  matrix  B(q)  can  be  computed  as  the  torque  vector 
given  by  the  algorithm  of  Fig.  7.14  with  gQ  =  0,  q  =  0,  (f  =  1  and  q.j  =  0 
for  j  i;  iterating  this  procedure  for  i  =  1 , ,n  leads  to  constructing  the 
matrix  B{q).  Hence,  from  the  current  values  of  B(q)  and  T'(q,q),  and  the 
given  r,  the  equations  in  (7.115)  can  be  integrated  as  illustrated  above. 

Solving  the  inverse  dynamics  problem  is  useful  for  manipulator  trajectory 
planning  and  control  algorithm  implementation.  Once  a  joint  trajectory  is 
specified  in  terms  of  positions,  velocities  and  accelerations  (typically  as  a  re¬ 
sult  of  an  inverse  kinematics  procedure),  and  if  the  end-effector  forces  are 
known,  inverse  dynamics  allows  computation  of  the  torques  to  be  applied  to 
the  joints  to  obtain  the  desired  motion.  This  computation  turns  out  to  be 
useful  both  for  verifying  feasibility  of  the  imposed  trajectory  and  for  com¬ 
pensating  nonlinear  terms  in  the  dynamic  model  of  a  manipulator.  To  this 
end,  Newton-Euler  formulation  provides  a  computationally  efficient  recursive 
method  for  on-line  computation  of  inverse  dynamics.  Nevertheless,  it  can  be 
shown  that  also  Lagrange  formulation  is  liable  to  a  computationally  efficient 
recursive  implementation,  though  with  a  nonnegligible  reformulation  effort. 

For  an  n-joint  manipulator  the  number  of  operations  required  is:8 


See  Sect.  E.l  for  the  definition  of  computational  complexity  of  an  algorithm. 
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•  0(n2)  for  computing  direct  dynamics , 

•  0(n )  for  computing  inverse  dynamics. 


7.7  Dynamic  Scaling  of  Trajectories 

The  existence  of  dynamic  constraints  to  be  taken  into  account  for  trajectory 
generation  has  been  mentioned  in  Sect.  4.1.  In  practice,  with  reference  to  the 
given  trajectory  time  or  path  shape  (segments  with  high  curvature),  the  tra¬ 
jectories  that  can  be  obtained  with  any  of  the  previously  illustrated  methods 
may  impose  too  severe  dynamic  performance  for  the  manipulator.  A  typical 
case  is  that  when  the  required  torques  to  generate  the  motion  are  larger  than 
the  maximum  torques  the  actuators  can  supply.  In  this  case,  an  infeasible 
trajectory  has  to  be  suitably  time-scaled. 

Suppose  a  trajectory  has  been  generated  for  all  the  manipulator  joints 
as  q(t),  for  t  £  [0 ,tf].  Computing  inverse  dynamics  allows  the  evaluation  of 
the  time  history  of  the  torques  r(t)  required  for  the  execution  of  the  given 
motion.  By  comparing  the  obtained  torques  with  the  torque  limits  available 
at  the  actuators,  it  is  easy  to  check  whether  or  not  the  trajectory  is  actually 
executable.  The  problem  is  then  to  seek  an  automatic  trajectory  dynamic 
scaling  technique  —  avoiding  inverse  dynamics  recomputation  —  so  that  the 
manipulator  can  execute  the  motion  on  the  specified  path  with  a  proper  timing 
law  without  exceeding  the  torque  limits. 

Consider  the  manipulator  dynamic  model  as  given  in  (7.42)  with  Fv  = 
O,  Fs  =  O  and  he  =  0,  for  simplicity.  The  term  C(q,  q)  accounting  for 
centrifugal  and  Coriolis  forces  has  a  quadratic  dependence  on  joint  velocities, 
and  thus  it  can  be  formally  rewritten  as 

C{q,q)q  =  r(q)[qq\,  (7.117) 

where  [qq]  indicates  the  symbolic  notation  of  the  (n(n+  l)/2  x  1)  vector 

[qq]  =  [q \  qi qi  ■  ■■  qn-idn  ql}T ; 

r(q)  is  a  proper  (n  x  n(n  +  l)/2)  matrix  that  satisfies  (7.117).  In  view  of  such 
position,  the  manipulator  dynamic  model  can  be  expressed  as 

B{q(t))q(t )  +  r(q(t))[q(t)q(t)]  +  g{q(t))  =  r(f),  (7.118) 

where  the  explicit  dependence  on  time  t  has  been  shown. 

Consider  the  new  variable  q(r(t))  satisfying  the  equation 

q(t)  =  q(r(t)),  (7.119) 


where  r(t)  is  a  strictly  increasing  scalar  function  of  time  with  r(0)  =  0  and 

*"(*/)  =  if- 
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Differentiating  (7.119)  twice  with  respect  to  time  provides  the  following 
relations: 


q  =  fq'(r)  (7.120) 

q  =  f2q"{r)  +  rq'  (r)  (7.121) 

where  the  prime  denotes  the  derivative  with  respect  to  r.  Substituting  (7.120), 
(7.121)  into  (7.118)  yields 

r2(B{q(r))q"(r)  +  r(q(r))[q'(r)q,(r)]')  +  rB(q(r))q'(r)  +  g(q(r))  =  r. 

(7.122) 

In  (7.118)  it  is  possible  to  identify  the  term 

rs(t)  =  B(q(t))q(t )  +  r(q(t))[q(t)q(t)],  (7.123) 

representing  the  torque  contribution  that  depends  on  velocities  and  accelera¬ 
tions.  Correspondingly,  in  (7.122)  one  can  set 

rs(t)  =  f2(B{q(r))q"(r)  +  r(q(r))[q' (r)q' (r)]J  +  r  B  (q(r))q'  (r) .  (7.124) 

By  analogy  with  (7.123),  it  can  be  written 

rs(r)  =  B(q(r))q"(r)  +  r(q(r))[q'(r)q'(r )]  (7.125) 

and  then  (7.124)  becomes 

Ts(t)  =  r2rs(r )  +  fB(q(r))q'(r).  (7.126) 

The  expression  in  (7.126)  gives  the  relationship  between  the  torque  contribu¬ 
tions  depending  on  velocities  and  accelerations  required  by  the  manipulator 
when  this  is  subject  to  motions  having  the  same  path  but  different  timing 
laws,  obtained  through  a  time  scaling  of  joint  variables  as  in  (7.119). 

Gravitational  torques  have  not  been  considered,  since  they  are  a  function 
of  the  joint  positions  only,  and  thus  their  contribution  is  not  influenced  by 
time  scaling. 

The  simplest  choice  for  the  scaling  function  r(t)  is  certainly  the  linear 
function 

r(t)  =  ct 

with  c  a  positive  constant.  In  this  case,  (7.126)  becomes 

rs{t)  =  c2fs(ct), 

which  reveals  that  a  linear  time  scaling  by  c  causes  a  scaling  of  the  magnitude 
of  the  torques  by  the  coefficient  c2.  Let  c  >  1:  (7.119)  shows  that  the  trajectory 
described  by  q(r(t)),  assuming  r  =  ct  as  the  independent  variable,  has  a 
duration  tf  >  tf  to  cover  the  entire  path  specified  by  q.  Correspondingly,  the 
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torque  contributions  fs(ct )  computed  as  in  (7.125)  are  scaled  by  the  factor  c2 
with  respect  to  the  torque  contributions  Ts(t)  required  to  execute  the  original 
trajectory  q(t). 

With  the  use  of  a  recursive  algorithm  for  inverse  dynamics  computation, 
it  is  possible  to  check  whether  the  torques  exceed  the  allowed  limits  during 
trajectory  execution;  obviously,  limit  violation  should  not  be  caused  by  the 
sole  gravity  torques.  It  is  necessary  to  find  the  joint  for  which  the  torque 
has  exceeded  the  limit  more  than  the  others,  and  to  compute  the  torque 
contribution  subject  to  scaling,  which  in  turn  determines  the  factor  c2.  It 
is  then  possible  to  compute  the  time-scaled  trajectory  as  a  function  of  the 
new  time  variable  r  =  ct  which  no  longer  exceeds  torque  limits.  It  should  be 
pointed  out,  however,  that  with  this  kind  of  linear  scaling  the  entire  trajectory 
may  be  penalized,  even  when  a  torque  limit  on  a  single  joint  is  exceeded  only 
for  a  short  interval  of  time. 


7.8  Operational  Space  Dynamic  Model 

As  an  alternative  to  the  joint  space  dynamic  model,  the  equations  of  motion 
of  the  system  can  be  expressed  directly  in  the  operational  space;  to  this  end  it 
is  necessary  to  find  a  dynamic  model  which  describes  the  relationship  between 
the  generalized  forces  acting  on  the  manipulator  and  the  number  of  minimal 
variables  chosen  to  describe  the  end-effector  position  and  orientation  in  the 
operational  space. 

Similar  to  kinematic  description  of  a  manipulator  in  the  operational  space, 
the  presence  of  redundant  DOFs  and/or  kinematic  and  representation  singu¬ 
larities  deserves  careful  attention  in  the  derivation  of  an  operational  space 
dynamic  model. 

The  determination  of  the  dynamic  model  with  Lagrange  formulation  using 
operational  space  variables  allows  a  complete  description  of  the  system  motion 
only  in  the  case  of  a  nonredundant  manipulator,  when  the  above  variables 
constitute  a  set  of  generalized  coordinates  in  terms  of  which  the  kinetic  energy, 
the  potential  energy,  and  the  nonconservative  forces  doing  work  on  them  can 
be  expressed. 

This  way  of  proceeding  does  not  provide  a  complete  description  of  dy¬ 
namics  for  a  redundant  manipulator;  in  this  case,  in  fact,  it  is  reasonable  to 
expect  the  occurrence  of  internal  motions  of  the  structure  caused  by  those 
joint  generalized  forces  which  do  not  affect  the  end-effector  motion. 

To  develop  an  operational  space  model  which  can  be  adopted  for  both 
redundant  and  nonredundant  manipulators,  it  is  then  convenient  to  start  from 
the  joint  space  model  which  is  in  all  general.  In  fact,  solving  (7.42)  for  the  joint 
accelerations,  and  neglecting  the  joint  friction  torques  for  simplicity,  yields 

q  =  -B~\q)C{q,  q)q  -  B~l{q)g(q)  +  B~\q)  JT(q)he  ~  he),  (7127) 
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where  the  joint  torques  t  have  been  expressed  in  terms  of  the  equivalent  end- 
effector  forces  7  according  to  (3.111).  It  is  worth  noting  that  h  represents  the 
contribution  of  the  end-effector  forces  due  to  contact  with  the  environment, 
whereas  7  expresses  the  contribution  of  the  end-effector  forces  due  to  joint 
actuation. 

On  the  other  hand,  the  second-order  differential  kinematics  equation 
in  (3.98)  describes  the  relationship  between  joint  space  and  operational  space 
accelerations,  i.e., 

xe  =  JA(q)q  +  JA{q,  q)q 

The  solution  in  (7.127)  features  the  geometric  Jacobian  J,  whereas  the  analyt¬ 
ical  Jacobian  J  a  appears  in  (3.98).  For  notation  uniformity,  in  view  of  (3.66), 
one  can  set 

T  A(xe)l/e  =  1  a  T  A(xe)he  =  Ha  (7.128) 

where  T a  is  the  transformation  matrix  between  the  two  Jacobians.  Substi¬ 
tuting  (7.127)  into  (3.98)  and  accounting  for  (7.128)  gives 

xe  =  -JAB  lCq  -  JAB  xg  +  JAq+  JaB~1Ja('ya  -  hA).  (7.129) 
where  the  dependence  on  q  and  q  has  been  omitted.  With  the  positions 


Ba  =  (JaB-'J^)-1  (7.130) 

CAxe  =  BaJ  aB  1Cq  —  B  AJ  Aq  (7.131) 

gA  =  BAJAB~1g,  (7.132) 

the  expression  in  (7.129)  can  be  rewritten  as 

B  A{,xe]xe  +  C  A  (xe ,  xe')  xe  T  gA{x^j  7  A  (7.133) 


which  is  formally  analogous  to  the  joint  space  dynamic  model  (7.42).  Notice 
that  the  matrix  J  aB  1  is  invertible  if  and  only  if  J  A  is  full-rank,  that  is, 
in  the  absence  of  both  kinematic  and  representation  singularities. 

For  a  nonredundant  manipulator  in  a  nonsingular  configuration,  the  ex¬ 
pressions  in  (7.130)-(7.132)  become: 

Ba  =  J~aTBJa (7.134) 
CAxe  =  JATCq  -  BAJAq  (7.135) 

gA  =  JATg.  (7.136) 

As  anticipated  above,  the  main  feature  of  the  obtained  model  is  its  formal 
validity  also  for  a  redundant  manipulator,  even  though  the  variables  xe  do 
not  constitute  a  set  of  generalized  coordinates  for  the  system;  in  this  case,  the 
matrix  BA  is  representative  of  a  kinetic  pseudo-energy. 

In  the  following,  the  utility  of  the  operational  space  dynamic  model 
in  (7.133)  for  solving  direct  and  inverse  dynamics  problems  is  investigated.  The 
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following  derivation  is  meaningful  for  redundant  manipulators;  for  a  nonre- 
dundant  manipulator,  in  fact,  using  (7.133)  does  not  pose  specific  problems 
as  long  as  J  A  is  nonsingular  ((7.134)-(7.136)). 

With  reference  to  operational  space,  the  direct  dynamics  problem  consists 
of  determining  the  resulting  end-effector  accelerations  xe(t )  (and  thus  xe(t), 
xe(t))  from  the  given  joint  torques  r(t)  and  end-effector  forces  he{t).  For  a 
redundant  manipulator,  (7.133)  cannot  be  directly  used,  since  (3.111)  has  a 
solution  in  7e  only  if  r  £  7 Z(JT).  It  follows  that  for  simulation  purposes, 
the  solution  to  the  problem  is  naturally  obtained  in  the  joint  space;  in  fact, 
the  expression  in  (7.42)  allows  the  computation  of  q ,  q ,  q  which,  substituted 
into  the  direct  kinematics  equations  in  ((2.82),  (3.62),  (3.98),  give  xe ,  xe,  xe, 
respectively. 

Formulation  of  an  inverse  dynamics  problem  in  the  operational  space  re¬ 
quires  the  determination  of  the  joint  torques  r(t)  that  are  needed  to  generate 
a  specific  motion  assigned  in  terms  of  xe(t ),  xe(t),  xe(t),  for  given  end-effector 
forces  he(t).  A  possible  way  of  solution  is  to  solve  a  complete  inverse  kinemat¬ 
ics  problem  for  (2.82),  (3.62),  (3.98),  and  then  compute  the  required  torques 
with  the  joint  space  inverse  dynamics  as  in  (7.42).  Hence,  for  redundant  ma¬ 
nipulators,  redundancy  resolution  is  performed  at  kinematic  level. 

An  alternative  solution  to  the  inverse  dynamics  problem  consists  of  com¬ 
puting  jA  as  in  (7.133)  and  the  joint  torques  r  as  in  (3.111).  In  this  way, 
however,  the  presence  of  redundant  DOFs  is  not  exploited  at  all,  since  the 
computed  torques  do  not  generate  internal  motions  of  the  structure. 

If  it  is  desired  to  find  a  formal  solution  that  allows  redundancy  resolution 
at  dynamic  level,  it  is  necessary  to  determine  those  torques  corresponding  to 
the  equivalent  end-effector  forces  computed  as  in  (7.133).  By  analogy  with 
the  differential  kinematics  solution  (3.54),  the  expression  of  the  torques  to  be 
determined  will  feature  the  presence  of  a  minimum-norm  term  and  a  homoge¬ 
neous  term.  Since  the  joint  torques  have  to  be  computed,  it  is  convenient  to 
express  the  model  (7.133)  in  terms  of  q ,  q ,  q.  By  recalling  the  positions  (7.131), 
(7.132),  the  expression  in  (7.133)  becomes 

BA{xe  -  JAq)  +  BAJAB  xCq  +  B AJ AB  lg  =  7 A-hA 
and,  in  view  of  (3.98), 

BAJAq  +  BAJAB  Cq  +  BAJAB  g  =  ~yA~hA.  (7.137) 

By  setting 

JA(q)  =  B-\q)JTA(q)BA(q),  (7.138) 

the  expression  in  (7.137)  becomes 

JA{BQ  +  Cq  +  g)  =  ~(A  -  hA-  (7.139) 

At  this  point,  from  the  joint  space  dynamic  model  in  (7.42),  it  is  easy  to 
recognize  that  (7.139)  can  be  written  as 

J a(t  ~  JAhA )  =7 A~hA 
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from  which 

JTat  =  1a-  (7-140) 

The  general  solution  to  (7.140)  is  of  the  form  (see  Problem  7.11) 

T  =  JTa{q)1a  +  -  JTA{q)jTA{q))T0,  (7.141) 

that  can  be  derived  by  observing  that  in  (7.138)  is  a  right  pseudo-inverse 

of  J 4  weighted  by  the  inverse  of  the  inertia  matrix  B  .  The  (n  x  1)  vector  of 

arbitrary  torques  tq  in  (7.141)  does  not  contribute  to  the  end-effector  forces, 

-  T 

since  it  is  projected  in  the  null  space  of  J A. 

To  summarize,  for  given  xe,  xe,  xe  and  hA ,  the  expression  in  (7.133) 
allows  the  computation  of  jA.  Then,  (7.141)  gives  the  torques  r  which,  besides 
executing  the  assigned  end-effector  motion,  generate  internal  motions  of  the 
structure  to  be  employed  for  handling  redundancy  at  dynamic  level  through 
a  suitable  choice  of  tq. 


7.9  Dynamic  Manipulability  Ellipsoid 

The  availability  of  the  dynamic  model  allows  formulation  of  the  dynamic  ma¬ 
nipulability  ellipsoid  which  provides  a  useful  tool  for  manipulator  dynamic 
performance  analysis.  This  can  be  used  for  mechanical  structure  design  as 
well  as  for  seeking  optimal  manipulator  configurations. 

Consider  the  set  of  joint  torques  of  constant  (unit)  norm 

ttt  =  1  (7.142) 

describing  the  points  on  the  surface  of  a  sphere.  It  is  desired  to  describe  the 
operational  space  accelerations  that  can  be  generated  by  the  given  set  of  joint 
torques. 

For  studying  dynamic  manipulability,  suppose  to  consider  the  case  of  a 
manipulator  standing  still  (q  =  0),  not  in  contact  with  the  environment  (he  = 
0).  The  simplified  model  is 


B{q)q  +  g{q)  =  T.  (7.143) 

The  joint  accelerations  q  can  be  computed  from  the  second-order  differen¬ 
tial  kinematics  that  can  be  obtained  by  differentiating  (3.39),  and  imposing 
successively  q  =  0,  leading  to 


ve  =  J{q)q.  (7.144) 

Solving  for  minimum-norm  accelerations  only,  for  a  nonsingular  Jacobian ,  and 
substituting  in  (7.143)  yields  the  expression  of  the  torques 


t  =  B(q)Jf(q)ve  +  g(q) 


(7.145) 
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Fig.  7.15.  Effect  of  gravity  on  the  dynamic  manipulability  ellipsoid  for  a  three-link 
planar  arm 


needed  to  derive  the  ellipsoid.  In  fact,  substituting  (7.145)  into  (7.142)  gives 

(B(q)j\q)ve  +  g(q))T  (B(q)Jf(q)ve  +  g(q))  =  1. 

The  vector  on  the  right-hand  side  of  (7.145)  can  be  rewritten  as 

BJ^ve+g  =  B{tfve  +  B~lg)  (7.146) 

=  B{J^ve  +  B~1g  +  J^JB  lg  -  J^JB^g) 

=  B(j^ve  +  J^JB^g  +  (In  -  J^J)B~1g) , 

where  the  dependence  on  q  has  been  omitted.  According  to  what  was  done 
for  solving  (7.144),  one  can  neglect  the  contribution  of  the  accelerations  given 
by  B  1  g  which  are  in  the  null  space  of  J  and  then  produce  no  end-effector 
acceleration.  Hence,  (7.146)  becomes 

BJft)e  +  g  =  BJ^  (ve  +  JB~lg)  (7.147) 

and  the  dynamic  manipulability  ellipsoid  can  be  expressed  in  the  form 

(ve  +  JB~1g)T  J^T  BT  Bj\i)e  +  JB~1g)  =  1.  (7.148) 

The  core  of  the  quadratic  form  J^T BT BJ^  depends  on  the  geometrical  and 
inertial  characteristics  of  the  manipulator  and  determines  the  volume  and 
principal  axes  of  the  ellipsoid.  The  vector  — JB  1  g ,  describing  the  contribu¬ 
tion  of  gravity,  produces  a  constant  translation  of  the  centre  of  the  ellipsoid 
(for  each  manipulator  configuration)  with  respect  to  the  origin  of  the  reference 
frame;  see  the  example  in  Fig.  7.15  for  a  three-link  planar  arm. 

The  meaning  of  the  dynamic  manipulability  ellipsoid  is  conceptually  simi¬ 
lar  to  that  of  the  ellipsoids  considered  with  reference  to  kineto-statics  duality. 
In  fact,  the  distance  of  a  point  on  the  surface  of  the  ellipsoid  from  the  end- 
effector  gives  a  measure  of  the  accelerations  which  can  be  imposed  to  the 
end-effector  along  the  given  direction,  with  respect  to  the  constraint  (7.142). 
With  reference  to  Fig.  7.15,  it  is  worth  noticing  how  the  presence  of  gravity 
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acceleration  allows  the  execution  of  larger  accelerations  downward,  as  natural 
to  predict. 

In  the  case  of  a  nonredundant  manipulator,  the  ellipsoid  reduces  to 

(ve  +  JB1g)TJ-TBTBJ~1(ve  +  JB  xg)  =  1.  (7.149) 
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Problems 

7.1.  Find  the  dynamic  model  of  a  two-link  Cartesian  arm  in  the  case  when 
the  second  joint  axis  forms  an  angle  of  7r/4  with  the  first  joint  axis;  compare 
the  result  with  the  model  of  the  manipulator  in  Fig.  7.3. 

7.2.  For  the  two-link  planar  arm  of  Sect.  7.3.2,  prove  that  with  a  different 
choice  of  the  matrix  C,  (7.49)  holds  true  while  (7.48)  does  not. 
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Fig.  7.16.  Two-link  planar  arm  with  a  prismatic  joint  and  a  revolute  joint 

7.3.  Find  the  dynamic  model  of  the  SCARA  manipulator  in  Fig.  2.36. 

7.4.  For  the  planar  arm  of  Sect.  7.3.2,  find  a  minimal  parameterization  of  the 
dynamic  model  in  (7.82). 

7.5.  Find  the  dynamic  model  of  the  two-link  planar  arm  with  a  prismatic 
joint  and  a  revolute  joint  in  Fig.  7.16  with  the  Lagrange  formulation.  Then, 
consider  the  addition  of  a  concentrated  tip  payload  of  mass  itil,  and  express 
the  resulting  model  in  a  linear  form  with  respect  to  a  suitable  set  of  dynamic 
parameters  as  in  (7.81). 

7.6.  For  the  two-link  planar  arm  of  Fig.  7.4,  find  the  dynamic  model  with 
the  Lagrange  formulation  when  the  absolute  angles  with  respect  to  the  base 
frame  are  chosen  as  generalized  coordinates.  Discuss  the  result  in  view  of  a 
comparison  with  the  model  derived  in  (7.82). 

7.7.  Compute  the  joint  torques  for  the  two-link  planar  arm  of  Fig.  7.4  with 
the  data  and  along  the  trajectories  of  Example  7.2,  in  the  case  of  tip  forces 
f  =  [500  500  ]TN. 

7.8.  Find  the  dynamic  model  of  the  two-link  planar  arm  with  a  prismatic 
joint  and  a  revolute  joint  in  Fig.  7.16  by  using  the  recursive  Newton-Euler 
algorithm. 

7.9.  For  the  two-link  planar  arm  of  Example  7.2,  perform  a  computer  im¬ 
plementation  of  the  dynamic  scaling  technique  with  a  linear  scaling  function 
for  the  trajectory  of  Fig.  7.5,  assuming  for  the  two  joints  the  torque  limit  of 
3000  N-m.  Adopt  a  sampling  time  of  1ms. 

7.10.  Show  that  for  the  operational  space  dynamic  model  (7.133)  a  skew- 
symmetry  property  holds  which  is  analogous  to  (7.48). 

7.11.  Show  how  to  obtain  the  general  solution  to  (7.140)  in  the  form  (7.141). 

7.12.  For  a  nonredundant  manipulator,  compute  the  relationship  between  the 
dynamic  manipulability  measure  that  can  be  defined  for  the  dynamic  manip- 
ulability  ellipsoid  and  the  manipulability  measure  defined  in  (3.56). 
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In  Chap.  4,  trajectory  planning  techniques  have  been  presented  which  al¬ 
low  the  generation  of  the  reference  inputs  to  the  motion  control  system.  The 
problem  of  controlling  a  manipulator  can  be  formulated  as  that  to  determine 
the  time  history  of  the  generalized  forces  (forces  or  torques)  to  be  developed 
by  the  joint  actuators,  so  as  to  guarantee  execution  of  the  commanded  task 
while  satisfying  given  transient  and  steady-state  requirements.  The  task  may 
regard  either  the  execution  of  specified  motions  for  a  manipulator  operating 
in  free  space,  or  the  execution  of  specified  motions  and  contact  forces  for  a 
manipulator  whose  end-effector  is  constrained  by  the  environment.  In  view  of 
problem  complexity,  the  two  aspects  will  be  treated  separately;  first,  motion 
control  in  free  space,  and  then  control  of  the  interaction  with  the  environ¬ 
ment.  The  problem  of  motion  control  of  a  manipulator  is  the  topic  of  this 
chapter.  A  number  of  joint  space  control  techniques  are  presented.  These  can 
be  distinguished  between  decentralized  control  schemes,  i.e.,  when  the  single 
manipulator  joint  is  controlled  independently  of  the  others,  and  centralized 
control  schemes,  i.e.,  when  the  dynamic  interaction  effects  between  the  joints 
are  taken  into  account.  Finally,  as  a  premise  to  the  interaction  control  prob¬ 
lem,  the  basic  features  of  operational  space  control  schemes  are  illustrated. 


8.1  The  Control  Problem 

Several  techniques  can  be  employed  for  controlling  a  manipulator.  The  tech¬ 
nique  followed,  as  well  as  the  way  it  is  implemented,  may  have  a  significant 
influence  on  the  manipulator  performance  and  then  on  the  possible  range  of 
applications.  For  instance,  the  need  for  trajectory  tracking  control  in  the  op¬ 
erational  space  may  lead  to  hardware/software  implementations,  which  differ 
from  those  allowing  point-to-point  control,  where  only  reaching  of  the  final 
position  is  of  concern. 

On  the  other  hand,  the  manipulator  mechanical  design  has  an  influence 
on  the  kind  of  control  scheme  utilized.  For  instance,  the  control  problem  of 
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Fig.  8.1.  General  scheme  of  joint  space  control 


a  Cartesian  manipulator  is  substantially  different  from  that  of  an  anthropo¬ 
morphic  manipulator. 

The  driving  system  of  the  joints  also  has  an  effect  on  the  type  of  control 
strategy  used.  If  a  manipulator  is  actuated  by  electric  motors  with  reduction 
gears  of  high  ratios,  the  presence  of  gears  tends  to  linearize  system  dynam¬ 
ics,  and  thus  to  decouple  the  joints  in  view  of  the  reduction  of  nonlinearity 
effects.  The  price  to  pay,  however,  is  the  occurrence  of  joint  friction,  elastic¬ 
ity  and  backlash  that  may  limit  system  performance  more  than  it  is  due  to 
configuration-dependent  inertia,  Coriolis  and  centrifugal  forces,  and  so  forth. 
On  the  other  hand,  a  robot  actuated  with  direct  drives  eliminates  the  draw¬ 
backs  due  to  friction,  elasticity  and  backlash,  but  the  weight  of  nonlinearities 
and  couplings  between  the  joints  becomes  relevant.  As  a  consequence,  different 
control  strategies  have  to  be  thought  of  to  obtain  high  performance. 

Without  any  concern  to  the  specific  type  of  mechanical  manipulator,  it 
is  worth  remarking  that  task  specification  (end-effector  motion  and  forces)  is 
usually  carried  out  in  the  operational  space,  whereas  control  actions  (joint 
actuator  generalized  forces)  are  performed  in  the  joint  space.  This  fact  nat¬ 
urally  leads  to  considering  two  kinds  of  general  control  schemes,  namely,  a 
joint  space  control  scheme  (Fig.  8.1)  and  an  operational  space  control  scheme 
(Fig.  8.2).  In  both  schemes,  the  control  structure  has  closed  loops  to  exploit 
the  good  features  provided  by  feedback,  i.e.,  robustness  to  modelling  uncer¬ 
tainties  and  reduction  of  disturbance  effects.  In  general  terms,  the  following 
considerations  should  be  made. 

The  joint  space  control  problem  is  actually  articulated  in  two  subprob¬ 
lems.  First,  manipulator  inverse  kinematics  is  solved  to  transform  the  motion 
requirements  xd  from  the  operational  space  into  the  corresponding  motion  qd 
in  the  joint  space.  Then,  a  joint  space  control  scheme  is  designed  that  allows 
the  actual  motion  q  to  track  the  reference  inputs.  However,  this  solution  has 
the  drawback  that  a  joint  space  control  scheme  does  not  influence  the  opera¬ 
tional  space  variables  xe  which  are  controlled  in  an  open-loop  fashion  through 
the  manipulator  mechanical  structure.  It  is  then  clear  that  any  uncertainty  of 
the  structure  (construction  tolerance,  lack  of  calibration,  gear  backlash,  elas¬ 
ticity)  or  any  imprecision  in  the  knowledge  of  the  end-effector  pose  relative 
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Fig.  8.2.  General  scheme  of  operational  space  control 


to  an  object  to  manipulate  causes  a  loss  of  accuracy  on  the  operational  space 
variables. 

The  operational  space  control  problem  follows  a  global  approach  that  re¬ 
quires  a  greater  algorithmic  complexity;  notice  that  inverse  kinematics  is  now 
embedded  into  the  feedback  control  loop.  Its  conceptual  advantage  regards  the 
possibility  of  acting  directly  on  operational  space  variables;  this  is  somewhat 
only  a  potential  advantage,  since  measurement  of  operational  space  variables 
is  often  performed  not  directly,  but  through  the  evaluation  of  direct  kinematics 
functions  starting  from  measured  joint  space  variables. 

On  the  above  premises,  in  the  following,  joint  space  control  schemes  for 
manipulator  motion  in  the  free  space  are  presented  first.  In  the  sequel,  op¬ 
erational  space  control  schemes  will  be  illustrated  which  are  logically  at  the 
basis  of  control  of  the  interaction  with  the  environment. 


8.2  Joint  Space  Control 

In  Chap.  7,  it  was  shown  that  the  equations  of  motion  of  a  manipulator  in 
the  absence  of  external  end-effector  forces  and,  for  simplicity,  of  static  friction 
(difficult  to  model  accurately)  are  described  by 

B(q)q  + C(q,q)q  + Fvq  + g(q)  =  r  (8.1) 

with  obvious  meaning  of  the  symbols.  To  control  the  motion  of  the  manipula¬ 
tor  in  free  space  means  to  determine  the  n  components  of  generalized  forces  — 
torques  for  re  volute  joints,  forces  for  prismatic  joints  —  that  allow  execution 
of  a  motion  q(t)  so  that 

<?0)  =  qd(t), 

as  closely  as  possible,  where  qd(t)  denotes  the  vector  of  desired  joint  trajectory 
variables. 

The  generalized  forces  are  supplied  by  the  actuators  through  proper  trans¬ 
missions  to  transform  the  motion  characteristics.  Let  qrn  denote  the  vector 
of  joint  actuator  displacements;  the  transmissions  —  assumed  to  be  rigid  and 
with  no  backlash  —  establish  the  following  relationship: 

K,q  =  qm,  (8.2) 
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Fig.  8.3.  Block  scheme  of  the  manipulator  and  drives  system  as  a  voltage-controlled 
system 


where  K,  is  an  (n  x  n)  diagonal  matrix,  whose  elements  are  defined  in  (7.22) 
and  are  much  greater  than  unity.1 

In  view  of  (8.2),  if  rm  denotes  the  vector  of  actuator  driving  torques,  one 
can  write 

rm  =  K~xt.  (8.3) 

With  reference  to  (5.1)-(5.4),  the  n  driving  systems  can  be  described  in 
compact  matrix  form  by  the  equations: 

K~xt  =  Ktia  (8.4) 

Va  Ra^a  4“  KvQm  (8.5) 

va  =  Gvvc.  (8.6) 

In  (8.4),  Kt  is  the  diagonal  matrix  of  torque  constants  and  ia  is  the  vector 
of  armature  currents  of  the  n  motors;  in  (8.5),  va  is  the  vector  of  armature 
voltages,  Ra  is  the  diagonal  matrix  of  armature  resistances,2  and  Kv  is  the 
diagonal  matrix  of  voltage  constants  of  the  n  motors;  in  (8.6),  Gv  is  the 
diagonal  matrix  of  gains  of  the  n  amplifiers  and  vc  is  the  vector  of  control 
voltages  of  the  n  servomotors. 

On  reduction  of  (8.1),  (8.2),  (8.4),  (8.5),  (8.6),  the  dynamic  model  of  the 


system  given  by  the  manipulator  and  drives  is  described  by 

B(q)q  +  C(q,q)q  +  Fq  +  g(q)  =u  (8.7) 

where  the  following  positions  have  been  made: 

F  =  FV  +  KrKtR^KyKr  (8.8) 

u  =  KrKtR~xGvvc.  (8.9) 

From  (8.1),  (8.7),  (8.8),  (8.9)  it  is 

KrKtRaxGvvc  =  t  +  KrKtR^KyKrq  (8.10) 


1  Assuming  a  diagonal  Kr  leads  to  excluding  the  presence  of  kinematic  couplings 
in  the  transmission,  that  is  the  motion  of  each  actuator  does  not  induce  motion 
on  a  joint  other  than  that  actuated. 

2  The  contribution  of  the  inductance  has  been  neglected. 
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Kr  Kt  Gt 


Fig.  8.4.  Block  scheme  of  the  manipulator  and  drives  system  as  a  torque-controlled 
system 


and  thus 

T  =  KrKtR~\Gvvc-KvKrq).  (8.11) 

The  overall  system  is  then  voltage-controlled  and  the  corresponding  block 
scheme  is  illustrated  in  Fig.  8.3.  If  the  following  assumptions  hold: 

•  the  elements  of  matrix  Kr ,  characterizing  the  transmissions,  are  much 
greater  than  unity; 

•  the  elements  of  matrix  Ra  are  very  small,  which  is  typical  in  the  case  of 
high-efficiency  servomotors; 

•  the  values  of  the  torques  r  required  for  the  execution  of  the  desired  motions 
are  not  too  large; 

then  it  can  be  assumed  that 


Gvvc  ss  KvK,  q.  (8-12) 

The  proportionality  relationship  obtained  between  q  and  vc  is  independent 
of  the  values  attained  by  the  manipulator  parameters;  the  smaller  the  joint 
velocities  and  accelerations,  the  more  valid  this  assumption.  Hence,  velocity 
(or  voltage)  control  shows  an  inherent  robustness  with  respect  to  parameter 
variations  of  the  manipulator  model,  which  is  enhanced  by  the  values  of  the 
gear  reduction  ratios. 

In  this  case,  the  scheme  illustrated  in  Fig.  8.3  can  be  taken  as  the  reference 
structure  for  the  design  of  the  control  system.  Having  assumed  that 

vc  «  G~1KvKrq  (8.13) 

implies  that  the  velocity  of  the  i-tli  joint  depends  only  on  the  i-tli  control  volt¬ 
age,  since  the  matrix  G~xKvKr  is  diagonal.  Therefore,  the  joint  position 
control  system  can  be  designed  according  to  a  decentralized  control  structure, 
since  each  joint  can  be  controlled  independently  of  the  others.  The  results, 
evaluated  in  the  terms  of  the  tracking  accuracy  of  the  joint  variables  with 
respect  to  the  desired  trajectories,  are  improved  in  the  case  of  higher  gear  re¬ 
duction  ratios  and  less  demanding  values  of  required  speeds  and  accelerations. 

On  the  other  hand,  if  the  desired  manipulator  motion  requires  large  joint 
speeds  and/or  accelerations,  the  approximation  (8.12)  no  longer  holds,  in  view 
of  the  magnitude  of  the  required  driving  torques;  this  occurrence  is  even  more 
evident  for  direct-drive  actuation  (Kr  =  I). 
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In  this  case,  by  resorting  to  an  inverse  dynamics  technique,  it  is  possible 
to  find  the  joint  torques  r(t)  needed  to  track  any  specified  motion  in  terms  of 
the  joint  accelerations  q(t),  velocities  q{t)  and  positions  q{t).  Obviously,  this 
solution  requires  the  accurate  knowledge  of  the  manipulator  dynamic  model. 
The  determination  of  the  torques  to  be  generated  by  the  drive  system  can  thus 
refer  to  a  centralized  control  structure,  since  to  compute  the  torque  history  at 
the  i-tli  joint  it  is  necessary  to  know  the  time  evolution  of  the  motion  of  all 
the  joints.  By  recalling  that 


r  =  KrKtia,  (8.14) 

to  find  a  relationship  between  the  torques  t  and  the  control  voltages  vc, 
using  (8.5),  (8.6)  leads  to 


r  =  KrKtR-lGvvc  -  KrKtR~1KvKrq.  (8.15) 

If  the  actuators  have  to  provide  torque  contributions  computed  on  the  basis 
of  the  manipulator  dynamic  model,  the  control  voltages  —  to  be  determined 
according  to  (8.15)  -  depend  on  the  torque  values  and  also  on  the  joint 
velocities;  this  relationship  depends  on  the  matrices  Kt,  Kv  and  R~  ,  whose 
elements  are  influenced  by  the  operating  conditions  of  the  motors.  To  reduce 
sensitivity  to  parameter  variations,  it  is  worth  considering  driving  systems 
characterized  by  a  current  control  rather  than  by  a  voltage  control.  In  this  case 
the  actuators  behave  as  torque-controlled  generators;  the  equation  in  (8.5) 
becomes  meaningless  and  is  replaced  by 

ia  =  G,vc,  (8.16) 

which  gives  a  proportional  relation  between  the  armature  currents  ia  (and 
thus  the  torques  t)  and  the  control  voltages  vc  established  by  the  constant 
matrix  G;.  As  a  consequence,  (8.9)  becomes 

t  =  u  =  KrKtGiVc  (8.17) 

which  shows  a  reduced  dependence  of  u  on  the  motor  parameters.  The  overall 
system  is  now  torque- controlled  and  the  resulting  block  scheme  is  illustrated 
in  Fig.  8.4. 

The  above  presentation  suggests  resorting  for  the  decentralized  structure 
—  where  the  need  for  robustness  prevails  —  to  feedback  control  systems,  while 
for  the  centralized  structure  —  where  the  computation  of  inverse  dynamics  is 
needed  —  it  is  necessary  to  refer  to  control  systems  with  feedforward  actions. 
Nevertheless,  it  should  be  pointed  out  that  centralized  control  still  requires 
the  use  of  error  contributions  between  the  desired  and  the  actual  trajectory, 
no  matter  whether  they  are  implemented  in  a  feedback  or  in  a  feedforward 
fashion.  This  is  a  consequence  of  the  fact  that  the  considered  dynamic  model, 
even  though  a  quite  complex  one,  is  anyhow  an  idealization  of  reality  which 


8.3  Decentralized  Control  309 


does  not  include  effects,  such  as  joint  Coulomb  friction,  gear  backlash,  di¬ 
mension  tolerance,  and  the  simplifying  assumptions  in  the  model,  e.g.,  link 
rigidity,  and  so  on. 

As  already  pointed  out,  the  drive  systems  is  anyhow  inserted  into  a  feed¬ 
back  control  system.  In  the  case  of  decentralized  control,  the  drive  will  be 
characterized  by  the  model  describing  its  behaviour  as  a  velocity-controlled 
generator.  Instead,  in  the  case  of  centralized  control,  since  the  driving  torque 
is  to  be  computed  on  a  complete  or  reduced  manipulator  dynamic  model,  the 
drive  will  be  characterized  as  a  torque-controlled  generator. 


8.3  Decentralized  Control 

The  simplest  control  strategy  that  can  be  thought  of  is  one  that  regards  the 
manipulator  as  formed  by  n  independent  systems  (the  n  joints)  and  con¬ 
trols  each  joint  axis  as  a  single-input/ single- output  system.  Coupling  effects 
between  joints  due  to  varying  configurations  during  motion  are  treated  as 
disturbance  inputs. 

In  order  to  analyze  various  control  schemes  and  their  performance,  it  is 
worth  considering  the  model  of  the  system  manipulator  with  drives  in  terms 
of  mechanical  quantities  at  the  motor  side;  in  view  of  (8.2),  (8.3),  it  is 

K^1  B(q)K~1qm  +  K~lC(q ,  q)K~x  qm  +  K~1FVK~1  +  K;lg(q)  =  rm. 

(8.18) 

By  observing  that  the  diagonal  elements  of  B{q)  are  formed  by  constant  terms 
and  configuration-dependent  terms  (functions  of  sine  and  cosine  for  revolute 
joints),  one  can  set 

B(q)  =  B  +  AB(q)  (8.19) 

where  B  is  the  diagonal  matrix  whose  constant  elements  represent  the  result¬ 
ing  average  inertia  at  each  joint.  Substituting  (8.19)  into  (8.1)  yields 

K~1BK~1qm  +  Fmqm  +  d  =  Tm  (8.20) 

where 

Fm  =  K~x  F  VK~X  (8.21) 

represents  the  matrix  of  viscous  friction  coefficients  about  the  motor  axes,  and 

d  =  K~1AB(q)K~1qm  +  K;lC(q.  q)K~lqm  +  K~lg{q)  (8.22) 

represents  the  contribution  depending  on  the  configuration. 

As  illustrated  by  the  block  scheme  of  Fig.  8.5,  the  system  of  manipulator 
with  drives  is  actually  constituted  by  two  subsystems;  one  has  Tm  as  input 
and  qm  as  output,  the  other  has  qm,  qm,  qm  as  inputs,  and  d  as  output.  The 
former  is  linear  and  decoupled,  since  each  component  of  rm  influences  only  the 
corresponding  component  of  qm.  The  latter  is  nonlinear  and  coupled,  since 
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NONLINEAR 

COUPLED 


Fig.  8.5.  Block  scheme  of  the  system  of  manipulator  with  drives 


it  accounts  for  all  those  nonlinear  and  coupling  terms  of  manipulator  joint 
dynamics. 

On  the  basis  of  the  above  scheme,  several  control  algorithms  can  be  derived 
with  reference  to  the  detail  of  knowledge  of  the  dynamic  model.  The  simplest 
approach  that  can  be  followed,  in  case  of  high-gear  reduction  ratios  and/or 
limited  performance  in  terms  of  required  velocities  and  accelerations,  is  to 
consider  the  component  of  the  nonlinear  interacting  term  d  as  a  disturbance 
for  the  single  joint  servo. 

The  design  of  the  control  algorithm  leads  to  a  decentralized  control  struc¬ 
ture,  since  each  joint  is  considered  independently  of  the  others.  The  joint 
controller  must  guarantee  good  performance  in  terms  of  high  disturbance  re¬ 
jection  and  enhanced  trajectory  tracking  capabilities.  The  resulting  control 
structure  is  substantially  based  on  the  error  between  the  desired  and  actual 
output,  while  the  input  control  torque  at  actuator  i  depends  only  on  the  error 
of  output  i. 

Therefore,  the  system  to  control  is  Joint  i  drive  corresponding  to  the  single- 
input/single-output  system  of  the  decoupled  and  linear  part  of  the  scheme  in 
Fig.  8.5.  The  interaction  with  the  other  joints  is  described  by  component  i  of 
the  vector  d  in  (8.22). 
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Fig.  8.6.  Block  scheme  of  general  independent  joint  control 


Assumed  that  the  actuator  is  a  rotary  electric  DC  motor,  the  general 
scheme  of  drive  control  is  that  in  Fig.  5.9  where  Im  is  the  average  inertia 
reported  to  the  motor  axis  (/m*  =  bu/k ^).3 

8.3.1  Independent  Joint  Control 

To  guide  selection  of  the  controller  structure,  start  noticing  that  an  effective 
rejection  of  the  disturbance  d  on  the  output  is  ensured  by: 

•  a  large  value  of  the  amplifier  gain  before  the  point  of  intervention  of  the 
disturbance, 

•  the  presence  of  an  integral  action  in  the  controller  so  as  to  cancel  the  effect 
of  the  gravitational  component  on  the  output  at  steady  state  (constant 

■dm)- 

These  requisites  clearly  suggest  the  use  of  a  proportional-integral  (PI)  con¬ 
trol  action  in  the  forward  path  whose  transfer  function  is 

1  4-  s  T 

C(s)  =  Kc — — - (8.23) 

s 

this  yields  zero  error  at  steady  state  for  a  constant  disturbance,  and  the  pres¬ 
ence  of  the  real  zero  at  s  =  —1/TC  offers  a  stabilizing  action.  To  improve 
dynamic  performance,  it  is  worth  choosing  the  controller  as  a  cascade  of  ele¬ 
mentary  actions  with  local  feedback  loops  closed  around  the  disturbance. 

Besides  closure  of  a  position  feedback  loop,  the  most  general  solution  is 
obtained  by  closing  inner  loops  on  velocity  and  acceleration.  This  leads  to 
the  scheme  in  Fig.  8.6,  where  Cp(s),  C'y(s),  Ca(s )  respectively  represent 
position,  velocity,  acceleration  controllers,  and  the  inmost  controller  should 


3  Subscript  i  is  to  be  dropped  for  notation  compactness. 
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be  of  PI  type  as  in  (8.23)  so  as  to  obtain  zero  error  at  steady  state  for  a 
constant  disturbance.  Further,  kpp,  kpv>  kpA  are  the  respective  transducer 
constants,  and  the  amplifier  gain  Gv  has  been  embedded  in  the  gain  of  the 
inmost  controller.  In  the  scheme  of  Fig.  8.6,  notice  that  $r  is  the  reference 
input,  which  is  related  to  the  desired  output  dmd  as 

$r  =  kpp'&md- 

Further,  the  disturbance  torque  D  has  been  suitably  transformed  into  a  volt¬ 
age  by  the  factor  Ra/kt. 

In  the  following,  a  number  of  possible  solutions  that  can  be  derived  from 
the  general  scheme  of  Fig.  8.6  are  presented;  at  this  stage,  the  issue  arising 
from  possible  lack  of  measurement  of  physical  variables  is  not  considered  yet. 
Three  case  studies  are  considered  which  differ  in  the  number  of  active  feedback 
loops.4 

Position  feedback 

In  this  case,  the  control  action  is  characterized  by 

Cp(s )  =  Kp - —  Cv{s)  =  1  CA(s)  =  1 

s 

kpv  =  kpA  =  0. 

With  these  positions,  the  structure  of  the  control  scheme  in  Fig.  8.6  leads  to 
the  scheme  illustrated  in  Fig.  5.10.  From  this  scheme  the  transfer  function  of 
the  forward  path  is 

kmKP(l  +  sTP) 

P{s)  =  - 2n  .  rp  \  > 

s2(l  +  sTm) 

while  that  of  the  return  path  is 


H{s)  =  kpp- 

A  root  locus  analysis  can  be  performed  as  a  function  of  the  gain  of  the  po¬ 
sition  loop  kmKpkTpTp/Tm.  Three  situations  are  illustrated  for  the  poles 
of  the  closed-loop  system  with  reference  to  the  relation  between  Tp  and  Tm 
(Fig.  8.7).  Stability  of  the  closed-loop  feedback  system  imposes  some  con¬ 
straints  on  the  choice  of  the  parameters  of  the  PI  controller.  If  Tp  <  Tm, 
the  system  is  inherently  unstable  (Fig.  8.7a).  Then,  it  must  be  Tp  >  Tm 
(Fig.  8.7b).  As  TP  increases,  the  absolute  value  of  the  real  part  of  the  two 
roots  of  the  locus  tending  towards  the  asymptotes  increases  too,  and  the  sys¬ 
tem  has  faster  time  response.  Hence,  it  is  convenient  to  render  Tp  Tm 
(Fig.  8.7c).  In  any  case,  the  real  part  of  the  dominant  poles  cannot  be  less 
than  —1/2  Tm. 

4  See  Appendix  C  for  a  brief  brush-up  on  control  of  linear  single-input /single-output 
systems. 
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Fig.  8.7.  Root  loci  for  the  position  feedback  control  scheme 


The  closed-loop  input /output  transfer  function  is 

Om(s) 


1 

krp 


Or(s) 


1  + 


S2(l  +  sTm) 


(8.24) 


kmKpkrp{  1  +  sTP) 


which  can  be  expressed  in  the  form 


W{s)  = 


7 - (1  +  sTp) 

kpp 


1  +  —  +  )  (!  +  sr) 


LUn 


UJi 


where  ton  and  /  are  respectively  the  natural  frequency  and  damping  ratio  of 
the  pair  of  complex  poles  and  — 1/r  locates  the  real  pole.  These  values  are 
assigned  to  define  the  joint  drive  dynamics  as  a  function  of  the  constant  Tp; 
if  TP  >  Tm,  then  l/i ^un  >  TP  >  r  (Fig.  8.7b);  if  TP  Tm  (Fig.  8.7c),  for 
large  values  of  the  loop  gain,  then  (cun  >  1/r  w  1/Tp  and  the  zero  at  —1/Tp 
in  the  transfer  function  W(s)  tends  to  cancel  the  effect  of  the  real  pole. 


314 


8  Motion  Control 


The  closed- loop  disturbance/output  transfer  function  is 

sRa 

Om(s)  _  ktKPkTp{l  +  sTp)  .  . 

D(s)  ~  1  |  s2(l  +  sTm)  ’  j 

kmKpkTp{  1  +  sTp) 

which  shows  that  it  is  worth  increasing  K p  to  reduce  the  effect  of  disturbance 
on  the  output  during  the  transient.  The  function  in  (8.25)  has  two  complex 
poles  {—Quin,  ±;j\/ 1  —  Q2uin),  a  real  pole  (— 1/r),  and  a  zero  at  the  origin.  The 
zero  is  due  to  the  PI  controller  and  allows  the  cancellation  of  the  effects  of 
gravity  on  the  angular  position  when  $m  is  a  constant. 

In  (8.25),  it  can  be  recognized  that  the  term  Kpkpp  is  the  reduction 
factor  imposed  by  the  feedback  gain  on  the  amplitude  of  the  output  due  to 
disturbance;  hence,  the  quantity 


Xp  =  Kpk.pp  (8.26) 

can  be  interpreted  as  the  disturbance  rejection  factor,  which  in  turn  is  de¬ 
termined  by  the  gain  Kp.  However,  it  is  not  advisable  to  increase  Kp  too 
much,  because  small  damping  ratios  would  result  leading  to  unacceptable  os¬ 
cillations  of  the  output.  An  estimate  Tp  of  the  output  recovery  time  needed 
by  the  control  system  to  recover  the  effects  of  the  disturbance  on  the  angular 
position  can  be  evaluated  by  analyzing  the  modes  of  evolution  of  (8.25).  Since 
t  rts  TP,  such  estimate  is  expressed  by 


Tp  =  max 


(8.27) 


Position  and  velocity  feedback 

In  this  case,  the  control  action  is  characterized  by 

Cp(s )  =  Kp  CV(s)  =  Ky - —  Ca{s)  =  1 

s 

kpA  =  0; 

with  these  positions,  the  structure  of  the  control  scheme  in  Fig.  8.6  leads  to 
scheme  illustrated  in  Fig.  5.11.  To  carry  out  a  root  locus  analysis  as  a  function 
of  the  velocity  feedback  loop  gain,  it  is  worth  reducing  the  velocity  loop  in 
parallel  to  the  position  loop  by  following  the  usual  rules  for  moving  blocks. 
From  the  scheme  in  Fig.  5.11  the  transfer  function  of  the  forward  path  is 

p,  ,  _  kmKpKv{  1  +  sTv) 

_  s2(l  +  sTm)  ’ 
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Fig.  8.8.  Root  locus  for  the  position  and  velocity  feedback  control  scheme 


while  that  of  the  return  path  is 


H(s) 


krp 


krv 

Kpkfp 


The  zero  of  the  controller  at  s  =  —  1/TV  can  be  chosen  so  as  to  cancel  the 
effects  of  the  real  pole  of  the  motor  at  s  =  —  1  /Tm.  Then,  by  setting 


TV  =  I'm 


the  poles  of  the  closed-loop  system  move  on  the  root  locus  as  a  function  of  the 
loop  gain  kmKykTv >  as  shown  in  Fig.  8.8.  By  increasing  the  position  feedback 
gain  Kp,  it  is  possible  to  confine  the  closed-loop  poles  into  a  region  of  the 
complex  plane  with  large  absolute  values  of  the  real  part.  Then,  the  actual 
location  can  be  established  by  a  suitable  choice  of  Ky. 

The  closed-loop  input /output  transfer  function  is 


1 

6*m(s)  kpp 

Or(s )  ,  skpv  s 2 

1  H - - - 1 - - - — 

Kpkpp  kmKpkppKv 


(8.28) 


which  can  be  compared  with  the  typical  transfer  function  of  a  second-order 
system 

1 


W(s) 


krp 


^ n 


(8.29) 


It  can  be  recognized  that,  with  a  suitable  choice  of  the  gains,  it  is  possible  to 
obtain  any  value  of  natural  frequency  u>n  and  damping  ratio  £.  Hence,  if  u)n 
and  £  are  given  as  design  requirements,  the  following  relations  can  be  found: 


2C^'n 


k 


m 


Kykrv 


(8.30) 
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Jl 

KpkxpKy  =  — — .  (8.31) 

For  given  transducer  constants  kpp  and  krvi  once  Ky  has  been  chosen  to 
satisfy  (8.30),  the  value  of  Kp  is  obtained  from  (8.31). 

The  closed- loop  disturbance/output  transfer  function  is 

sRa 

Om(s)  _  ktKpkTpKy{\  +  sTm)  ,  . 

nf  \  7  2  ’  (o.oz) 

D{s)  1  |  skTy  | _ s 

Kpkpp  kmKpkppKy 

which  shows  that  the  disturbance  rejection  factor  is 


Xp  =  KpkppKy  (8.33) 

and  is  fixed,  once  Kp  and  Ky  have  been  chosen  via  (8.30),  (8.31).  Concerning 
disturbance  dynamics,  the  presence  of  a  zero  at  the  origin  introduced  by  the 
PI,  of  a  real  pole  at  s  =  —1/Tm,  and  of  a  pair  of  complex  poles  having  real 
part  —(u)n  should  be  noticed.  Hence,  in  this  case,  an  estimate  of  the  output 
recovery  time  is  given  by  the  time  constant 

Tm )7M;  (8.34) 

C  J 

which  reveals  an  improvement  with  respect  to  the  previous  case  in  (8.27), 
since  Tm  <C  Tp  and  the  real  part  of  the  dominant  poles  is  not  constrained  by 
the  inequality  fu>n  <  1/2 Tm. 


Tp  =  max 
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kmk.TvKvKA 


Fig.  8.10.  Root  locus  for  the  position,  velocity  and  acceleration  feedback  control 
scheme 


Position,  velocity  and  acceleration  feedback 

In  this  case,  the  control  action  is  characterized  by 


CP (s)  =  KP  Cv{s)  =  Kv  CA(s)  =  Ka  1  +  sTa  . 

s 

After  some  manipulation,  the  block  scheme  of  Fig.  8.6  can  be  reduced  to  that 
of  Fig.  8.9  where  G'(s)  indicates  the  following  transfer  function: 


G'(s)  = 


(1  +  km  K A  a) 


1  + 


V 


sTm  ^1  +  kmK Akr A-jp-^  \ 
(1  +  kjn  Ka  kp  a  ) 


The  transfer  function  of  the  forward  path  is 

F(„)  =  g-W  +  »r,)o, 


while  that  of  the  return  path  is 


Also  in  this  case,  a  suitable  pole  cancellation  is  worthy  which  can  be  achieved 
either  by  setting 

Ta  =  Tm, 


or  by  making 


kmKAkTATA  >  Tm 


k"m  Ka  kpA  >  1- 
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The  two  solutions  are  equivalent  as  regards  dynamic  performance  of  the  con¬ 
trol  system.  In  both  cases,  the  poles  of  the  closed-loop  system  are  constrained 
to  move  on  the  root  locus  as  a  function  of  the  loop  gain  kmKpKyK A/ (1  + 
kmKAkTA )  (Fig.  8.10).  A  close  analogy  with  the  previous  scheme  can  be 
recognized,  in  that  the  resulting  closed-loop  system  is  again  of  second-order 
type. 

The  closed-loop  input /output  transfer  function  is 


6>r(s) 


1  + 


1 

_ krp _ 

skTv  ,  s2(l  +  kmKAkTA)' 


Kpkpp  kmKpkTpKvKA 


while  the  closed- loop  disturbance/output  transfer  function  is 

sRa 

Om(s) 

D(s)  ^  SKTV 


1  + 


kt.K  pkp  p  Ky  K a(X  +  sTa) 
skTy  s2(l  +  kmKAkTA)' 


Kpkpp  kmKpkppKyKA 

The  resulting  disturbance  rejection  factor  is  given  by 


(8.35) 


(8.36) 


XR  =  KpkppKyKA, 


(8.37) 


while  the  output  recovery  time  is  given  by  the  time  constant 

Tr  =  max  | Ta,  j  (8-38) 

where  TA  can  be  made  less  than  Tm,  as  pointed  out  above. 

With  reference  to  the  transfer  function  in  (8.29),  the  following  relations 
can  be  established  for  design  purposes,  once  (,  uin,  XR  have  been  specified: 


2A  pkpp  ai  n 

kpy  C 

kmKAkTA=k^-  1 

W n 

KpkppKyKA  =  Xr. 


(8.39) 

(8.40) 

(8.41) 
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For  given  kpp ,  kpv ,  kpA,  Kp  is  chosen  to  satisfy  (8.39),  K  a  is  chosen  to 
satisfy  (8.40),  and  then  Ky  is  obtained  from  (8.41).  Notice  how  admissible 
solutions  for  the  controller  typically  require  large  values  for  the  rejection  fac¬ 
tor  Xp.  Hence,  in  principle,  not  only  does  the  acceleration  feedback  allow  the 
achievement  of  any  desired  dynamic  behaviour  but,  with  respect  to  the  pre¬ 
vious  case,  it  also  allows  the  prescription  of  the  disturbance  rejection  factor 
as  long  as  kmXR/ul  >  1. 

In  deriving  the  above  control  schemes,  the  issue  of  measurement  of  feed¬ 
back  variables  was  not  considered  explicitly.  With  reference  to  the  typical 
position  control  servos  that  are  implemented  in  industrial  practice,  there 
is  no  problem  of  measuring  position  and  velocity,  while  a  direct  measure¬ 
ment  of  acceleration,  in  general,  either  is  not  available  or  is  too  expensive  to 
obtain.  Therefore,  for  the  scheme  of  Fig.  8.9,  an  indirect  measurement  can 
be  obtained  by  reconstructing  acceleration  from  direct  velocity  measurement 
through  a  first-order  filter  (Fig.  8.11).  The  filter  is  characterized  by  a  band¬ 
width  u)3f  =  kf.  By  choosing  this  bandwidth  wide  enough,  the  effects  due 
to  measurement  lags  are  not  appreciable,  and  then  it  is  feasible  to  take  the 
acceleration  filter  output  as  the  quantity  to  feed  back.  Some  problem  may 
occur  concerning  the  noise  superimposed  on  the  filtered  acceleration  signal, 
though. 

Resorting  to  a  filtering  technique  may  be  useful  when  only  the  direct  posi¬ 
tion  measurement  is  available.  In  this  case,  by  means  of  a  second-order  state 
variable  filter,  it  is  possible  to  reconstruct  velocity  and  acceleration.  However, 
the  greater  lags  induced  by  the  use  of  a  second-order  filter  typically  degrade 
the  performance  with  respect  to  the  use  of  a  first-order  filter,  because  of  lim¬ 
itations  imposed  on  the  filter  bandwidth  by  numerical  implementation  of  the 
controller  and  filter. 

Notice  that  the  above  derivation  is  based  on  an  ideal  dynamic  model,  i.e. , 
when  the  effects  of  transmission  elasticity  as  well  as  those  of  amplifier  and 
motor  electrical  time  constants  are  neglected.  This  implies  that  satisfaction 
of  design  requirements  imposing  large  values  of  feedback  gains  may  not  be 
verified  in  practice,  since  the  existence  of  unmodelled  dynamics  —  such  as 
electric  dynamics,  elastic  dynamics  due  to  non-perfectly  rigid  transmissions, 
filter  dynamics  for  the  third  scheme  —  might  lead  to  degrading  the  system  and 
eventually  driving  it  to  instability.  In  summary,  the  above  solutions  constitute 
design  guidelines  whose  limits  should  be  emphasized  with  regard  to  the  specific 
application. 

8.3.2  Decentralized  Feedforward  Compensation 

When  the  joint  control  servos  are  required  to  track  reference  trajectories  with 
high  values  of  speed  and  acceleration,  the  tracking  capabilities  of  the  scheme  in 
Fig.  8.6  are  unavoidably  degraded.  The  adoption  of  a  decentralized  feedforward 
compensation  allows  a  reduction  of  the  tracking  error.  Therefore,  in  view 
of  the  closed-loop  input/output  transfer  functions  in  (8.24),  (8.28),  (8.35), 
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Fig.  8.12.  Block  scheme  of  position  feedback  control  with  decentralized  feedforward 
compensation 


the  reference  inputs  to  the  three  control  structures  analyzed  in  the  previous 
section  can  be  respectively  modified  into 
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(8.42) 

(8.43) 

(8.44) 


in  this  way,  tracking  of  the  desired  joint  position  &md(s)  is  achieved,  if  not 
for  the  effect  of  disturbances.  Notice  that  computing  time  derivatives  of  the 
desired  trajectory  is  not  a  problem,  once  ’drnd,{t)  is  known  analytically.  The 
tracking  control  schemes,  resulting  from  simple  manipulation  of  (8.42),  (8.43), 
(8.44)  are  reported  respectively  in  Figs.  8.12,  8.13,  8.14,  where  M(s)  indicates 
the  motor  transfer  function  in  (5.11),  with  krn  and  Tm  as  in  (5.12). 

All  the  solutions  allow  the  input  trajectory  to  be  tracked  within  the  range 
of  validity  and  linearity  of  the  models  employed.  It  is  worth  noticing  that,  as 
the  number  of  nested  feedback  loops  increases,  a  less  accurate  knowledge  of 
the  system  model  is  required  to  perform  feedforward  compensation.  In  fact, 
Tm  and  km  are  required  for  the  scheme  of  Fig.  8.12,  only  km  is  required  for 
the  scheme  of  Fig.  8.13,  and  krn  again  but  with  reduced  weight  —  for  the 
scheme  of  Fig.  8.14. 

It  is  worth  recalling  that  perfect  tracking  can  be  obtained  only  under  the 
assumption  of  exact  matching  of  the  controller  and  feedforward  compensation 
parameters  with  the  process  parameters,  as  well  as  of  exact  modelling  and 
linearity  of  the  physical  system.  Deviations  from  the  ideal  values  cause  a 
performance  degradation  that  should  be  analyzed  case  by  case. 
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Fig.  8.13.  Block  scheme  of  position  and  velocity  feedback  control  with  decentralized 
feedforward  compensation 


®md 


Fig.  8.14.  Block  scheme  of  position,  velocity  and  acceleration  feedback  control  with 
decentralized  feedforward  compensation 


The  presence  of  saturation  blocks  in  the  schemes  of  Figs.  8.12,  8.13,  8.14 
is  to  be  intended  as  intentional  nonlinearities  whose  function  is  to  limit  rele¬ 
vant  physical  quantities  during  transients;  the  greater  the  number  of  feedback 
loops,  the  greater  the  number  of  quantities  that  can  be  limited  (velocity,  ac¬ 
celeration,  and  motor  voltage).  To  this  end,  notice  that  trajectory  tracking  is 
obviously  lost  whenever  any  of  the  above  quantities  saturates.  This  situation 
often  occurs  for  industrial  manipulators  required  to  execute  point-to-point 
motions;  in  this  case,  there  is  less  concern  about  the  actual  trajectories  fol¬ 
lowed,  and  the  actuators  are  intentionally  taken  to  operate  at  the  current 
limits  so  as  to  realize  the  fastest  possible  motions. 

After  simple  block  reduction  on  the  above  schemes,  it  is  possible  to  de¬ 
termine  equivalent  control  structures  that  utilize  position  feedback  only  and 
regulators  with  standard  actions.  It  should  be  emphasized  that  the  two  solu¬ 
tions  are  equivalent  in  terms  of  disturbance  rejection  and  trajectory  tracking. 
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Fig.  8.15.  Equivalent  control  scheme  of  PI  type 


However,  tuning  of  regulator  parameters  is  less  straightforward,  and  the  elim¬ 
ination  of  inner  feedback  loops  prevents  the  possibility  of  setting  saturations 
on  velocity  and/or  acceleration.  The  control  structures  equivalent  to  those 
of  Figs.  8.12,  8.13,  8.14  are  illustrated  in  Figs.  8.15,  8.16,  8.17,  respectively; 
control  actions  of  PI,  PID,  PIDD2  type  are  illustrated  which  are  respectively 
equivalent  to  the  cases  of:  position  feedback;  position  and  velocity  feedback; 
position,  velocity  and  acceleration  feedback. 

It  is  worth  noticing  that  the  equivalent  control  structures  in  Figs.  8.15-8.17 
are  characterized  by  the  presence  of  the  feedforward  action  ( Trn/krn)‘drnd  + 
(1  /kmj’dmd-  If  the  motor  is  current-controlled  and  not  voltage-controlled,  by 
recalling  (5.13),  the  feedforward  action  is  equal  to  {ki/kt){Imdmd  +  Fmdmd)- 
If  'dm  ~  firnd,  dm  ~  and  the  disturbance  is  negligible,  the  term  Imdd  + 
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Fig.  8.17.  Equivalent  control  scheme  of  PIDD2  type 


Fm'&d.  represents  the  driving  torque  providing  the  desired  velocity  and  accel¬ 
eration,  as  indicated  by  (5.3).  By  setting 

f ad  ^  T 

kt 

the  feedforward  action  can  be  rewritten  in  the  form  kiiad ■  This  shows  that,  in 
the  case  the  drive  is  current-controlled,  it  is  possible  to  replace  the  acceleration 
and  velocity  feedforward  actions  with  a  current  and  thus  a  torque  feedforward 
action,  which  is  to  be  properly  computed  with  reference  to  the  desired  motion. 

This  equivalence  is  illustrated  in  Fig.  8.18,  where  M(s)  has  been  replaced 
by  the  block  scheme  of  an  electric  drive  of  Fig.  5.2,  where  the  parameters  of 
the  current  loop  are  chosen  so  as  to  realize  a  torque-controlled  generator.  The 
feedforward  action  represents  a  reference  for  the  motor  current,  which  im¬ 
poses  the  generation  of  the  nominal  torque  to  execute  the  desired  motion;  the 
presence  of  the  position  reference  allows  the  closure  of  a  feedback  loop  which, 
in  view  of  the  adoption  of  a  standard  regulator  with  transfer  function  Cr(s), 
confers  robustness  to  the  presented  control  structure.  In  summary,  the  perfor¬ 
mance  that  can  be  achieved  with  velocity  and  acceleration  feedforward  actions 
and  voltage-controlled  actuator  can  be  achieved  with  a  current-controlled  ac¬ 
tuator  and  a  desired  torque  feedforward  action. 

The  above  schemes  can  incorporate  the  typical  structure  of  the  controllers 
actually  implemented  in  the  control  architectures  of  industrial  robots.  In  these 
systems  it  is  important  to  choose  the  largest  possible  gains  so  that  model 
inaccuracy  and  coupling  terms  do  not  appreciably  affect  positions  of  the  single 
joints.  As  pointed  out  above,  the  upper  limit  on  the  gains  is  imposed  by 
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Fig.  8.18.  Control  scheme  with  current-controlled  drive  and  current  feedforward 
action 

all  those  factors  that  have  not  been  modelled,  such  as  implementation  of 
discrete-time  controllers  in  lieu  of  the  continuous-time  controllers  analyzed 
in  theory,  presence  of  finite  sampling  time,  neglected  dynamic  effects  (e.g., 
joint  elasticity,  structural  resonance,  finite  transducer  bandwidth) ,  and  sensor 
noise.  In  fact,  the  influence  of  such  factors  in  the  implementation  of  the  above 
controllers  may  cause  a  severe  system  performance  degradation  for  much  too 
large  values  of  feedback  gains. 


8.4  Computed  Torque  Feedforward  Control 

Define  the  tracking  error  e(f)  =  $m(;(f)  —  dm(t).  With  reference  to  the  most 
general  scheme  (Fig.  8.17),  the  output  of  the  PIDD2  regulator  can  be  written 
as 


Cl2 &  ”h  CL\C  CLqC  CL— i 


;(<?)* 


which  describes  the  time  evolution  of  the  error.  The  constant  coefficients 
a2,ai,ao,a_i  are  determined  by  the  particular  solution  adopted.  Summing 
the  contribution  of  the  feedforward  actions  and  of  the  disturbance  to  this 
expression  yields 

Trn  -A  1  ■„  Ra 


T^dmd+-dmd-  ~^d, 
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The  input  to  the  motor  (Fig.  8.6)  has  then  to  satisfy  the  following  equation: 

a2e  +  a1e  +  a0e  +  a-1  (  e(c)d<T+  ^ d  =  +  77- 

J  rbm  fi'm  Kt  ^m  ^m 

With  a  suitable  change  of  coefficients,  this  can  be  rewritten  as 


I'^e  +  a^e  +  a'0e  +  a,_1  f  e(c)dc  =  — 1 -d. 

J  kt 
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This  equation  describes  the  error  dynamics  and  shows  that  any  physically 
executable  trajectory  is  asymptotically  tracked  only  if  the  disturbance  term 
d(t)  =  0.  With  the  term  physically  executable  it  is  meant  that  the  saturation 
limits  on  the  physical  quantities,  e.g.,  current  and  voltage  in  electric  motors, 
are  not  violated  in  the  execution  of  the  desired  trajectory. 

The  presence  of  the  term  d(t)  causes  a  tracking  error  whose  magnitude  is 
reduced  as  much  as  the  disturbance  frequency  content  is  located  off  to  the  left 
of  the  lower  limit  of  the  bandwidth  of  the  error  system.  The  disturbance/error 
transfer  function  is  given  by 

Ra 

E{s)  _  ~k 7S 

D(s )  a'2s3  +  a[s2  +  a'0s  +  a'_1  ’ 

and  thus  the  adoption  of  loop  gains  which  are  not  realizable  for  the  above 
discussed  reasons  is  often  required. 

Nevertheless,  even  if  the  term  d(t)  has  been  introduced  as  a  disturbance, 
its  expression  is  given  by  (8.22).  It  is  then  possible  to  add  a  further  term  to 
the  previous  feedforward  actions  which  is  able  to  compensate  the  disturbance 
itself  rather  than  its  effects.  In  other  words,  by  taking  advantage  of  model 
knowledge,  the  rejection  effort  of  an  independent  joint  control  scheme  can  be 
lightened  with  notable  simplification  from  the  implementation  viewpoint. 

Let  qd(t)  be  the  desired  joint  trajectory  and  qmd(t)  the  corresponding 
actuator  trajectory  as  in  (8.2).  By  adopting  an  inverse  model  strategy,  the 
feedforward  action  RaKf1dd  can  be  introduced  with 

dd  =  Kf1AB(qd)Kf1qmd  +  KfxC{qd,  qd)Kflqmd  +  K^gfa),  (8.45) 

where  Ra  and  Kt  denote  the  diagonal  matrices  of  armature  resistances  and 
torque  constants  of  the  actuators.  This  action  tends  to  compensate  the  actual 
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disturbance  expressed  by  (8.22)  and  in  turn  allows  the  control  system  to 
operate  in  a  better  condition. 

This  solution  is  illustrated  in  the  scheme  of  Fig.  8.19,  which  conceptually 
describes  the  control  system  of  a  manipulator  with  computed  torque  control. 
The  feedback  control  system  is  representative  of  the  n  independent  joint  con¬ 
trol  servos;  it  is  decentralized,  since  controller  i  elaborates  references  and  mea¬ 
surements  that  refer  to  single  Joint  i.  The  interactions  between  the  various 
joints,  expressed  by  d,  are  compensated  by  a  centralized  action  whose  function 
is  to  generate  a  feedforward  action  that  depends  on  the  joint  references  as  well 
as  on  the  manipulator  dynamic  model.  This  action  compensates  the  nonlinear 
coupling  terms  due  to  inertial,  Coriolis,  centrifugal,  and  gravitational  forces 
that  depend  on  the  structure  and,  as  such,  vary  during  manipulator  motion. 

Although  the  residual  disturbance  term  d  =  dd  —  d  vanishes  only  in  the 
ideal  case  of  perfect  tracking  ( q  =  qd)  and  exact  dynamic  modelling,  d  is 
representative  of  interaction  disturbances  of  considerably  reduced  magnitude 
with  respect  to  d.  Hence,  the  computed  torque  technique  has  the  advantage  to 
alleviate  the  disturbance  rejection  task  for  the  feedback  control  structure  and 
in  turn  allows  limited  gains.  Notice  that  expression  (8.45)  in  general  imposes  a 
computationally  demanding  burden  on  the  centralized  part  of  the  controller. 
Therefore,  in  those  applications  where  the  desired  trajectory  is  generated  in 
real  time  with  regard  to  exteroceptive  sensory  data  and  commands  from  higher 
hierarchical  levels  of  the  robot  control  architecture,  on-line  computation  of 
the  centralized  feedforward  action  may  require  too  much  time.5 6 

Since  the  actual  controller  is  to  be  implemented  on  a  computer  with  a 
finite  sampling  time,  torque  computation  has  to  be  carried  out  during  this 
interval  of  time;  in  order  not  to  degrade  dynamic  system  performance,  typical 
sampling  times  are  of  the  order  of  the  millisecond. 

Therefore,  it  may  be  worth  performing  only  a  partial  feedforward  action 
so  as  to  compensate  those  terms  of  (8.45)  that  give  the  most  relevant  con¬ 
tributions  during  manipulator  motion.  Since  inertial  and  gravitational  terms 
dominate  velocity-dependent  terms  (at  operational  joint  speeds  not  greater 
than  a  few  radians  per  second),  a  partial  compensation  can  be  achieved  by 
computing  only  the  gravitational  torques  and  the  inertial  torques  due  to  the 
diagonal  elements  of  the  inertia  matrix.  In  this  way,  only  the  terms  depending 
on  the  global  manipulator  configuration  are  compensated  while  those  deriving 
from  motion  interaction  with  the  other  joints  are  not. 

Finally,  it  should  be  pointed  out  that,  for  repetitive  trajectories,  the  above 
compensating  contributions  can  be  computed  off-line  and  properly  stored  on 
the  basis  of  a  trade-off  solution  between  memory  capacity  and  computational 
requirements  of  the  control  architecture. 


5  See  also  Chap.  6. 

6  In  this  regard,  the  problem  of  real-time  computation  of  compensating  torques  can 
be  solved  by  resorting  to  efficient  recursive  formulations  of  manipulator  inverse 
dynamics,  such  as  the  Newton-Euler  algorithm  presented  in  Chap.  7. 
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8.5  Centralized  Control 

In  the  previous  sections  several  techniques  have  been  discussed  that  allow 
the  design  of  independent  joint  controllers.  These  are  based  on  a  single- 
input/single-output  approach,  since  interaction  and  coupling  effects  between 
the  joints  have  been  considered  as  disturbances  acting  on  each  single  joint 
drive  system. 

On  the  other  hand,  when  large  operational  speeds  are  required  or  direct- 
drive  actuation  is  employed  (Kr  =  /),  the  nonlinear  coupling  terms  strongly 
influence  system  performance.  Therefore,  considering  the  effects  of  the  com¬ 
ponents  of  d  as  a  disturbance  may  generate  large  tracking  errors.  In  this  case, 
it  is  advisable  to  design  control  algorithms  that  take  advantage  of  a  detailed 
knowledge  of  manipulator  dynamics  so  as  to  compensate  for  the  nonlinear 
coupling  terms  of  the  model.  In  other  words,  it  is  necessary  to  eliminate  the 
causes  rather  than  to  reduce  the  effects  induced  by  them;  that  is,  to  generate 
compensating  torques  for  the  nonlinear  terms  in  (8.22).  This  leads  to  central¬ 
ized  control  algorithms  that  are  based  on  the  (partial  or  complete)  knowledge 
of  the  manipulator  dynamic  model. 

Whenever  the  robot  is  endowed  with  the  torque  sensors  at  the  joint  motors 
presented  in  Sect.  5.4.1,  those  measurements  can  be  conveniently  utilized  to 
generate  the  compensation  action,  thus  avoiding  the  on-line  computation  of 
the  terms  of  the  dynamic  model. 

As  shown  by  the  dynamic  model  (8.1),  the  manipulator  is  not  a  set  of 
n  decoupled  system  but  it  is  a  multivariable  system  with  n  inputs  (joint 
torques)  and  n  outputs  (joint  positions)  interacting  between  them  by  means 
of  nonlinear  relations.7 

In  order  to  follow  a  methodological  approach  which  is  consistent  with 
control  design,  it  is  necessary  to  treat  the  control  problem  in  the  context  of 
nonlinear  multivariable  systems.  This  approach  will  obviously  account  for  the 
manipulator  dynamic  model  and  lead  to  finding  nonlinear  centralized  control 
laws,  whose  implementation  is  needed  for  high  manipulator  dynamic  perfor¬ 
mance.  On  the  other  hand,  the  above  computed  torque  control  can  be  inter¬ 
preted  in  this  framework,  since  it  provides  a  model-based  nonlinear  control 
term  to  enhance  trajectory  tracking  performance.  Notice,  however,  that  this 
action  is  inherently  performed  off  line,  as  it  is  computed  on  the  time  history 
of  the  desired  trajectory  and  not  of  the  actual  one. 

In  the  following,  the  problem  of  the  determination  of  the  control  law  u 
ensuring  a  given  performance  to  the  system  of  manipulator  with  drives  is 
tackles.  Since  (8.17)  can  be  considered  as  a  proportional  relationship  between 
vc  and  it,  the  centralized  control  schemes  below  refer  directly  to  the  generation 
of  control  toques  u. 


7 


See  Appendix  C  for  the  basic  concepts  on  control  of  nonlinear  mechanical  systems. 
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8.5.1  PD  Control  with  Gravity  Compensation 

Let  a  constant  equilibrium  posture  be  assigned  for  the  system  as  the  vector  of 
desired  joint  variables  qd.  It  is  desired  to  find  the  structure  of  the  controller 
which  ensures  global  asymptotic  stability  of  the  above  posture. 

The  determination  of  the  control  input  which  stabilizes  the  system  around 
the  equilibrium  posture  is  based  on  the  Lyapunov  direct  method. 

Take  the  vector  [qT  qT]T  as  the  system  state,  where 

q  =  qd-Q  (8.46) 

represents  the  error  between  the  desired  and  the  actual  posture.  Choose  the 
following  positive  definite  quadratic  form  as  Lyapunov  function  candidate: 

V(q,q)  =  ^qTB(q)q+  ^qrKPq  >  0  Vq,  q  ^  0  (8.47) 

where  K  p  is  an  (n  x  n)  symmetric  positive  definite  matrix.  An  energy-based 
interpretation  of  (8.47)  reveals  a  first  term  expressing  the  system  kinetic  en¬ 
ergy  and  a  second  term  expressing  the  potential  energy  stored  in  the  system 
of  equivalent  stiffness  K p  provided  by  the  n  position  feedback  loops. 

Differentiating  (8.47)  with  respect  to  time,  and  recalling  that  qd  is  con¬ 
stant,  yields 

V  =  qT B(q)q  +  ^q1  B(q)q  -  qTKPq.  (8.48) 

Solving  (8.7)  for  Bq  and  substituting  it  in  (8.48)  gives 

V  =  ^ qT(B(q )  -2 C(q,q))q  -  qT Fq  +  qT (u  -  g{q)  -  KPq).  (8.49) 

The  first  term  on  the  right-hand  side  is  null  since  the  matrix  N  =  B  —  2 C 
satisfies  (7.49).  The  second  term  is  negative  definite.  Then,  the  choice 

u  =  g{q)+KPq ,  (8.50) 

describing  a  controller  with  compensation  of  gravitational  terms  and  a  pro¬ 
portional  action,  leads  to  a  negative  semi-definite  V  since 

P  =  0  q  =  0,\/q. 


This  result  can  be  obtained  also  by  taking  the  control  law 

u  =  g(q)  +  KPq  -  KDq,  (8.51) 

with  K p>  positive  definite,  corresponding  to  a  nonlinear  compensation  action 
of  gravitational  terms  with  a  linear  proportional-derivative  (PD)  action.  In 
fact,  substituting  (8.51)  into  (8.49)  gives 

V=  -qT(F  +  KD)q, 


(8.52) 
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Fig.  8.20.  Block  scheme  of  joint  space  PD  control  with  gravity  compensation 


which  reveals  that  the  introduction  of  the  derivative  term  causes  an  increase 
of  the  absolute  values  of  V  along  the  system  trajectories,  and  then  it  gives  an 
improvement  of  system  time  response.  Notice  that  the  inclusion  of  a  derivative 
action  in  the  controller,  as  in  (8.51),  is  crucial  when  direct-drive  manipulators 
are  considered.  In  that  case,  in  fact,  mechanical  viscous  damping  is  practi¬ 
cally  null,  and  current  control  does  not  allow  the  exploitation  of  the  electrical 
viscous  damping  provided  by  voltage-controlled  actuators. 

According  to  the  above,  the  function  candidate  V  decreases  as  long  as 
q  /  0  for  all  system  trajectories.  It  can  be  shown  that  the  system  reaches  an 
equilibrium  posture.  To  find  such  posture,  notice  that  V  =  0  only  if  q  =  0. 
System  dynamics  under  control  (8.51)  is  given  by 

B(q)q  +  C(q,  q)q  +  Fq  +  g(q)  =  g(q)  +  KPq  -  K Dq.  (8.53) 

At  the  equilibrium  (q  =  0,  q  =  0)  it  is 

KPq  =  0  (8.54) 


and  then 

<7  =  qd  -  q  =  0 

is  the  sought  equilibrium  posture.  The  above  derivation  rigorously  shows  that 
any  manipulator  equilibrium  posture  is  globally  asymptotically  stable  under 
a  controller  with  a  PD  linear  action  and  a  nonlinear  gravity  compensating 
action.  Stability  is  ensured  for  any  choice  of  K p  and  Kp,,  as  long  as  these  are 
positive  definite  matrices.  The  resulting  block  scheme  is  shown  in  Fig.  8.20. 

The  control  law  requires  the  on-line  computation  of  the  term  g(q).  If  com¬ 
pensation  is  imperfect,  the  above  discussion  does  not  lead  to  the  same  result; 
this  aspect  will  be  revisited  later  with  reference  to  robustness  of  controllers 
performing  nonlinear  compensation. 


330 


8  Motion  Control 


Fig.  8.21.  Exact  linearization  performed  by  inverse  dynamics  control 


8.5.2  Inverse  Dynamics  Control 

Consider  now  the  problem  of  tracking  a  joint  space  trajectory.  The  reference 
framework  is  that  of  control  of  nonlinear  multivariable  systems.  The  dynamic 
model  of  an  n-joint  manipulator  is  expressed  by  (8.7)  which  can  be  rewritten 
as 

B(q)q  +  n(q,q)  =u,  (8.55) 

where  for  simplicity  it  has  been  set 

n(q,q)  =  C{q,q)q  + Fq  + g(q).  (8.56) 

The  approach  that  follows  is  founded  on  the  idea  to  find  a  control  vector  u ,  as 
a  function  of  the  system  state,  which  is  capable  of  realizing  an  input /output 
relationship  of  linear  type;  in  other  words,  it  is  desired  to  perform  not  an 
approximate  linearization  but  an  exact  linearization  of  system  dynamics  ob¬ 
tained  by  means  of  a  nonlinear  state  feedback.  The  possibility  of  finding  such 
a  linearizing  controller  is  guaranteed  by  the  particular  form  of  system  dynam¬ 
ics.  In  fact,  the  equation  in  (8.55)  is  linear  in  the  control  u  and  has  a  full-rank 
matrix  B(q)  which  can  be  inverted  for  any  manipulator  configuration. 

Taking  the  control  u  as  a  function  of  the  manipulator  state  in  the  form 

u=  B{q)y  +  n(q,q),  (8.57) 

leads  to  the  system  described  by 


q  =  y 

where  y  represents  a  new  input  vector  whose  expression  is  to  be  determined 
yet;  the  resulting  block  scheme  is  shown  in  Fig.  8.21.  The  nonlinear  control 
law  in  (8.57)  is  termed  inverse  dynamics  control  since  it  is  based  on  the  com¬ 
putation  of  manipulator  inverse  dynamics.  The  system  under  control  (8.57) 
is  linear  and  decoupled  with  respect  to  the  new  input  y.  In  other  words,  the 
component  yi  influences,  with  a  double  integrator  relationship,  only  the  joint 
variable  qi ,  independently  of  the  motion  of  the  other  joints. 
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Fig.  8.22.  Block  scheme  of  joint  space  inverse  dynamics  control 


In  view  of  the  choice  (8.57),  the  manipulator  control  problem  is  reduced 
to  that  of  finding  a  stabilizing  control  law  y.  To  this  end,  the  choice 

y  =  -KPq-  KDq  +  r  (8.58) 

leads  to  the  system  of  second-order  equations 

q  +  KDq  +  KPq  =  r  (8.59) 

which,  under  the  assumption  of  positive  definite  matrices  Kp  and  Kp,  is 
asymptotically  stable.  Choosing  Kp  and  Kp  as  diagonal  matrices  of  the 
type 


KP  =  diag {wjb,  •  ■  •  ,^nnJ  kd  =  diag{2CiW„i, . . .  ,2(nunn}, 

gives  a  decoupled  system.  The  reference  component  influences  only  the  joint 
variable  qi  with  a  second-order  input/output  relationship  characterized  by  a 
natural  frequency  0Jnl  and  a  damping  ratio  Q. 

Given  any  desired  trajectory  qd(i),  tracking  of  this  trajectory  for  the  out¬ 
put  q(t)  is  ensured  by  choosing 

r  =  qd  + KDqd  + KPq(l.  (8.60) 

In  fact,  substituting  (8.60)  into  (8.59)  gives  the  homogeneous  second-order 
differential  equation 

q  +  KDq  +  KPq  =  0  (8.61) 

expressing  the  dynamics  of  position  error  (8.46)  while  tracking  the  given  tra¬ 
jectory.  Such  error  occurs  only  if  q( 0)  and/or  q( 0)  are  different  from  zero 
and  converges  to  zero  with  a  speed  depending  on  the  matrices  Kp  and  Kp 
chosen. 
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The  resulting  block  scheme  is  illustrated  in  Fig.  8.22,  in  which  two  feed¬ 
back  loops  are  represented;  an  inner  loop  based  on  the  manipulator  dynamic 
model,  and  an  outer  loop  operating  on  the  tracking  error.  The  function  of 
the  inner  loop  is  to  obtain  a  linear  and  decoupled  input/output  relationship, 
whereas  the  outer  loop  is  required  to  stabilize  the  overall  system.  The  con¬ 
troller  design  for  the  outer  loop  is  simplified  since  it  operates  on  a  linear  and 
time-invariant  system.  Notice  that  the  implementation  of  this  control  scheme 
requires  computation  of  the  inertia  matrix  B(q)  and  of  the  vector  of  Coriolis, 
centrifugal,  gravitational,  and  damping  terms  n(q,q )  in  (8.56).  Unlike  com¬ 
puted  torque  control,  these  terms  must  be  computed  on-line  since  control  is 
now  based  on  nonlinear  feedback  of  the  current  system  state,  and  thus  it  is 
not  possible  to  precompute  the  terms  off  line  as  for  the  previous  technique. 

The  above  technique  of  nonlinear  compensation  and  decoupling  is  very  at¬ 
tractive  from  a  control  viewpoint  since  the  nonlinear  and  coupled  manipulator 
dynamics  is  replaced  with  n  linear  and  decoupled  second-order  subsystems. 
Nonetheless,  this  technique  is  based  on  the  assumption  of  perfect  cancellation 
of  dynamic  terms,  and  then  it  is  quite  natural  to  raise  questions  about  sensi¬ 
tivity  and  robustness  problems  due  to  unavoidably  imperfect  compensation. 

Implementation  of  inverse  dynamics  control  laws  indeed  requires  that  pa¬ 
rameters  of  the  system  dynamic  model  are  accurately  known  and  the  complete 
equations  of  motion  are  computed  in  real  time.  These  conditions  are  difficult 
to  verify  in  practice.  On  one  hand,  the  model  is  usually  known  with  a  certain 
degree  of  uncertainty  due  to  imperfect  knowledge  of  manipulator  mechani¬ 
cal  parameters,  existence  of  unmodelled  dynamics,  and  model  dependence  on 
end-effector  payloads  not  exactly  known  and  thus  not  perfectly  compensated. 
On  the  other  hand,  inverse  dynamics  computation  is  to  be  performed  at  sam¬ 
pling  times  of  the  order  of  a  millisecond  so  as  to  ensure  that  the  assumption 
of  operating  in  the  continuous  time  domain  is  realistic.  This  may  pose  severe 
constraints  on  the  hardware/software  architecture  of  the  control  system.  In 
such  cases,  it  may  be  advisable  to  lighten  the  computation  of  inverse  dynamics 
and  compute  only  the  dominant  terms. 

On  the  basis  of  the  above  remarks,  from  an  implementation  viewpoint, 
compensation  may  be  imperfect  both  for  model  uncertainty  and  for  the  ap¬ 
proximations  made  in  on-line  computation  of  inverse  dynamics.  In  the  follow¬ 
ing,  two  control  techniques  are  presented  which  are  aimed  at  counteracting 
the  effects  of  imperfect  compensation.  The  first  consists  of  the  introduction  of 
an  additional  term  to  an  inverse  dynamics  controller  which  provides  robust¬ 
ness  to  the  control  system  by  counteracting  the  effects  of  the  approximations 
made  in  on-line  computation  of  inverse  dynamics.  The  second  adapts  the  pa¬ 
rameters  of  the  model  used  for  inverse  dynamics  computation  to  those  of  the 
true  manipulator  dynamic  model. 
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8.5.3  Robust  Control 

In  the  case  of  imperfect  compensation,  it  is  reasonable  to  assume  in  (8.55)  a 
control  vector  expressed  by 

u  =  B(q)y  +  n{q,q)  (8.62) 

where  B  and  n  represent  the  adopted  computational  model  in  terms  of  es¬ 
timates  of  the  terms  in  the  dynamic  model.  The  error  on  the  estimates,  i.e. , 
the  uncertainty ,  is  represented  by 

B  =  B  —  B  n  =  n  n  (8.63) 

and  is  due  to  imperfect  model  compensation  as  well  as  to  intentional  simplifi¬ 
cation  in  inverse  dynamics  computation.  Notice  that  by  setting  B  =  B  (where 
B  is  the  diagonal  matrix  of  average  inertia  at  the  joint  axes)  and  n  —  0,  the 
above  decentralized  control  scheme  is  recovered  where  the  control  action  y 
can  be  of  the  general  PID  type  computed  on  the  error. 

Using  (8.62)  as  a  nonlinear  control  law  gives 

Bq  +  n  =  By  +  n  (8.64) 

where  functional  dependence  has  been  omitted.  Since  the  inertia  matrix  B  is 
invertible,  it  is 

q  =  y  +  (B~1B  —  I)y  +  B -1n  =  y  —  rj  (8.65) 

where 

rj  =  (I  -  B  lB)y  -  B^n.  (8.66) 

Taking  as  above 

V  =  Qd  +  KD(qd  -q)  +  KP(qd  -  q), 

leads  to 

q  +  KDq  +  KPq  =  rj.  (8.67) 

The  system  described  by  (8.67)  is  still  nonlinear  and  coupled,  since  rj  is  a 
nonlinear  function  of  q  and  q;  error  convergence  to  zero  is  not  ensured  by  the 
term  on  the  left-hand  side  only. 

To  find  control  laws  ensuring  error  convergence  to  zero  while  tracking  a 
trajectory  even  in  the  face  of  uncertainties,  a  linear  PD  control  is  no  longer 
sufficient.  To  this  end,  the  Lyapunov  direct  method  can  be  utilized  again  for 
the  design  of  an  outer  feedback  loop  on  the  error  which  should  be  robust  to 
the  uncertainty  rj. 

Let  the  desired  trajectory  qd(t)  be  assigned  in  the  joint  space  and  let 
q  =  qd  —  q  be  the  position  error.  Its  first  time-derivative  is  q  =  qd  —  q,  while 
its  second  time-derivative  in  view  of  (8.65)  is 

q  =  <id-y  +  ri- 


(8.68) 


334 


8  Motion  Control 


By  taking 


(8.69) 


as  the  system  state,  the  following  first-order  differential  matrix  equation  is 
obtained: 


£  =  HS  +  D(qd-y  +  n),  (8.70) 


where  H  and 
respectively: 


D  are  block  matrices  of  dimensions  (2 n  x  2 n)  and  (2 n  x  n), 


H  = 


O 

O 


I 

o 


(8.71) 


Then,  the  problem  of  tracking  a  given  trajectory  can  be  regarded  as  the  prob¬ 
lem  of  finding  a  control  law  y  which  stabilizes  the  nonlinear  time- varying  error 
system  (8.70). 

Control  design  is  based  on  the  assumption  that,  even  though  the  uncer¬ 
tainty  7)  is  unknown,  an  estimate  on  its  range  of  variation  is  available.  The 
sought  control  law  y  should  guarantee  asymptotic  stability  of  (8.70)  for  any 
77  varying  in  the  above  range.  By  recalling  that  77  in  (8.66)  is  a  function  of  <7, 
<7,  qd,  the  following  assumptions  are  made: 


supt>0  \\qd\\  <QM  <00  Vqd  (8.72) 

\\I  ~  B-\q)B{q)W  <a<lV<7  (8.73) 


\\n\\  <  ^  <  00  V<7,  q.  (8.74) 

Assumption  (8.72)  is  practically  satisfied  since  any  planned  trajectory  cannot 
require  infinite  accelerations. 

Regarding  assumption  (8.73),  since  B  is  a  positive  definite  matrix  with 
upper  and  lower  limited  norms,  the  following  inequality  holds: 

0  <  Bm  <  || I?'1  (q)  ||  <  Bm  <  00  V9,  (8.75) 

and  then  a  choice  for  B  always  exists  which  satisfies  (8.73).  In  fact,  by  setting 


Bm  +  Bm 

from  (8.73)  it  is 

WB-'B-IW  <BM~  Bm  =  a  <1.  (8.76) 

n>M  + 

If  B  is  a  more  accurate  estimate  of  the  inertia  matrix,  the  inequality  is  satisfied 
with  values  of  a  that  can  be  made  arbitrarily  small  (in  the  limit,  it  is  B  =  B 
and  a  =  0). 
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Finally,  concerning  assumption  (8.74),  observe  that  n  is  a  function  of  q  and 
q.  For  revolute  joints  a  periodical  dependence  on  q  is  obtained,  while  for  pris¬ 
matic  joints  a  linear  dependence  is  obtained,  but  the  joint  ranges  are  limited 
and  then  the  above  contribution  is  also  limited.  On  the  other  hand,  regarding 
the  dependence  on  q ,  unbounded  velocities  for  an  unstable  system  may  arise 
in  the  limit,  but  in  reality  saturations  exist  on  the  maximum  velocities  of  the 
motors.  In  summary,  assumption  (8.74)  can  be  realistically  satisfied,  too. 
With  reference  to  (8.65),  choose  now 

y  =  qd  + KDq  + Kpq  +  w  (8.77) 

where  the  PD  term  ensures  stabilization  of  the  error  dynamic  system  matrix, 
qd  provides  a  feedforward  term,  and  the  term  w  is  to  be  chosen  to  guarantee 
robustness  to  the  effects  of  uncertainty  described  by  r)  in  (8.66). 

Using  (8.77)  and  setting  K  =  [Kp  Kp]  yields 

k  =  H^  +  D(rj-w),  (8.78) 

where 

H  =  {H  -  DK)  = 

is  a  matrix  whose  eigenvalues  all  have  negative  real  parts  -  K p  and  K p 
being  positive  definite  —  which  allows  the  desired  error  system  dynamics  to 
be  prescribed.  In  fact,  by  choosing  Kp  =  diag {lu^,  . . .  ,co%n}  and  Kp  = 
diag{2((iwr!i, . . . ,  2£nujnn},  n  decoupled  equations  are  obtained  as  regards  the 
linear  part.  If  the  uncertainty  term  vanishes,  it  is  obviously  w  =  0  and  the 
above  result  with  an  exact  inverse  dynamics  controller  is  recovered  (B  =  B 
and  n  =  n). 

To  determine  w ,  consider  the  following  positive  definite  quadratic  form  as 
Lyapunov  function  candidate: 

V(Z)  =  >0  V£  ^  0,  (8.79) 

where  Q  is  a  (2 n  x  2 n)  positive  definite  matrix.  The  derivative  of  V  along  the 
trajectories  of  the  error  system  (8.78)  is 

V  =  £TQ£  +  ^Qk  (8.80) 

=  ZT{HTQ  +  QH)i  +  2irQD{r1  w). 

Since  H  has  eigenvalues  with  all  negative  real  parts,  it  is  well-known  that  for 
any  symmetric  positive  definite  matrix  P,  the  equation 

HTQ  +  QH  =  -P  (8.81) 

gives  a  unique  solution  Q  which  is  symmetric  positive  definite  as  well.  In  view 
of  this,  (8.80)  becomes 


O  I 

KP  - Kp 


V  =  QD(r)  -  w). 


(8.82) 
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ROBUSTNESS 


Fig.  8.23.  Block  scheme  of  joint  space  robust  control 


The  first  term  on  the  right-hand  side  of  (8.82)  is  negative  definite  and  then 
the  solutions  converge  if  £  €  Af(DTQ).  If  instead  £  ^  Af(DTQ ),  the  control 
w  must  be  chosen  so  as  to  render  the  second  term  in  (8.82)  less  than  or  equal 
to  zero.  By  setting  z  =  Dr  the  second  term  in  (8.82)  can  be  rewritten  as 
zT(rj  —  w).  Adopting  the  control  law 

W  =  -rr—rrZ  p>  0  (8.83) 

llZll 

gives8 

zT(j]-w )  =  zTr1-^zTz 

<\\z\\M-p\\z\\ 

=  NKINI  -p).  (8.84) 

Then,  if  p  is  chosen  so  that 

P>\\V  II  Vq,q,qd,  (8.85) 

the  control  (8.83)  ensures  that  V  is  less  than  zero  along  all  error  system 
trajectories. 

In  order  to  satisfy  (8.85),  notice  that,  in  view  of  the  definition  of  r/  in  (8.66) 
and  of  assumptions  (8.72)-(8.74),  and  being  ||u?||  =  p,  it  is 

INI  <  11/ -  B-'BWiMA  +  \m  Hill  +  Ml)  +  IIS-1!!  IN 

8  Notice  that  it  is  necessary  to  divide  z  by  the  norm  of  z  so  as  to  obtain  a  linear 
dependence  on  z  of  the  term  containing  the  control  zTw,  and  thus  to  effectively 
counteract,  for  z  — >  0,  the  term  containing  the  uncertainty  zTr)  which  is  linear 


in  z. 
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<  chQm  +  o:||.K)|  ||l||  +  ap  +  Bm^- 
Therefore,  setting 


(8.86) 


P  > 


1  —  a 


(aQM  +  a\\K\\U\\  +  BM$) 


(8.87) 


gives 

V=-ePt  +  2zT  <0  VI  ^0.  (8.88) 

The  resulting  block  scheme  is  illustrated  in  Fig.  8.23. 

To  summarize,  the  presented  approach  has  lead  to  finding  a  control  law 

which  is  formed  by  three  different  contributions: 

•  The  term  By+n  ensures  an  approximate  compensation  of  nonlinear  effects 
and  joint  decoupling. 

•  The  term  qd  +  K pq  +  Kpq  introduces  a  linear  feedforward  action  ( qd  + 
K p>qd+K pqd)  and  linear  feedback  action  (— K  pq—K  pq )  which  stabilizes 
the  error  system  dynamics. 

•  The  term  w  =  (p/||z||);z  represents  the  robust  contribution  that  counter¬ 
acts  the  indeterminacy  B  and  n  in  computing  the  nonlinear  terms  that 
depend  on  the  manipulator  state;  the  greater  the  uncertainty,  the  greater 
the  positive  scalar  p.  The  resulting  control  law  is  of  the  unit  vector  type, 
since  it  is  described  by  a  vector  of  magnitude  p  aligned  with  the  unit  vector 
of  z  =  DtQ£,  V|. 


All  the  resulting  trajectories  under  the  above  robust  control  reach  the  sub¬ 
space  z  =  Dt  Q!  =  0  that  depends  on  the  matrix  Q  in  the  Lyapunov  function 
V.  On  this  attractive  subspace,  termed  sliding  subspace,  the  control  w  is  ide¬ 
ally  commuted  at  an  infinite  frequency  and  all  error  components  tend  to  zero 
with  a  transient  depending  on  the  matrices  Q ,  Kp,  Kp.  A  characterization 
of  an  error  trajectory  in  the  two-dimensional  case  is  given  in  Fig.  8.24.  Notice 
that  in  the  case  |(0)  ^  0,  with  |(0)  ^  N(DTQ),  the  trajectory  is  attracted 
on  the  sliding  hyperplane  (a  line)  z  —  0  and  tends  towards  the  origin  of  the 
error  state  space  with  a  time  evolution  governed  by  p. 

In  reality,  the  physical  limits  on  the  elements  employed  in  the  controller 
impose  a  control  signal  that  commutes  at  a  finite  frequency,  and  the  trajec¬ 
tories  oscillate  around  the  sliding  subspace  with  a  magnitude  as  low  as  the 
frequency  is  high. 

Elimination  of  these  high-frequency  components  ( chattering )  can  be  achie¬ 
ved  by  adopting  a  robust  control  law  which,  even  if  it  does  not  guarantee  error 
convergence  to  zero,  ensures  bounded-norm  errors.  A  control  law  of  this  type 


if 


IS 


w  = 


per  ||z||  >  e 
per  ||z||  <  e. 


(8.89) 
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In  order  to  provide  an  intuitive  interpretation  of  this  law,  notice  that  (8.89) 
gives  a  null  control  input  when  the  error  is  in  the  null  space  of  matrix  DT Q. 
On  the  other  hand,  (8.83)  has  an  equivalent  gain  tending  to  infinity  when  z 
tends  to  the  null  vector,  thus  generating  a  control  input  of  limited  magnitude. 
Since  these  inputs  commute  at  an  infinite  frequency,  they  force  the  error 
system  dynamics  to  stay  on  the  sliding  subspace.  With  reference  to  the  above 
example,  control  law  (8.89)  gives  rise  to  a  hyperplane  z  =  0  which  is  no 
longer  attractive,  and  the  error  is  allowed  to  vary  within  a  boundary  layer 
whose  thickness  depends  on  e  (Fig.  8.25). 

The  introduction  of  a  contribution  based  on  the  computation  of  a  suitable 
linear  combination  of  the  generalized  error  confers  robustness  to  a  control 
scheme  based  on  nonlinear  compensation.  Even  if  the  manipulator  is  accu¬ 
rately  modeled,  indeed,  an  exact  nonlinear  compensation  may  be  computa¬ 
tionally  demanding,  and  thus  it  may  require  either  a  sophisticated  hardware 
architecture  or  an  increase  of  the  sampling  time  needed  to  compute  the  con¬ 
trol  law.  The  solution  then  becomes  weak  from  an  engineering  viewpoint,  due 
either  to  infeasible  costs  of  the  control  architecture,  or  to  poor  performance 
at  decreased  sampling  rates.  Therefore,  considering  a  partial  knowledge  of  the 
manipulator  dynamic  model  with  an  accurate,  pondered  estimate  of  uncer¬ 
tainty  may  suggest  robust  control  solutions  of  the  kind  presented  above.  It 
is  understood  that  an  estimate  of  the  uncertainty  should  be  found  so  as  to 
impose  control  inputs  which  the  mechanical  structure  can  bear. 

8.5.4  Adaptive  Control 

The  computational  model  employed  for  computing  inverse  dynamics  typically 
has  the  same  structure  as  that  of  the  true  manipulator  dynamic  model,  but 
parameter  estimate  uncertainty  does  exist.  In  this  case,  it  is  possible  to  devise 
solutions  that  allow  an  on-line  adaptation  of  the  computational  model  to  the 
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Fig.  8.25.  Error  trajectory  with  robust  control  and  chattering  elimination 


dynamic  model,  thus  performing  a  control  scheme  of  the  inverse  dynamics 
type. 

The  possibility  of  finding  adaptive  control  laws  is  ensured  by  the  property 
of  linearity  in  the  parameters  of  the  dynamic  model  of  a  manipulator.  In 
fact,  it  is  always  possible  to  express  the  nonlinear  equations  of  motion  in  a 
linear  form  with  respect  to  a  suitable  set  of  constant  dynamic  parameters  as 
in  (7.81).  The  equation  in  (8.7)  can  then  be  written  as 

B(q)q  +  C(<?,  q)q  +  Fq  +  g(q)  =  Y (q,  q ,  q)r v  =  u,  (8.90) 

where  tv  is  a  (p  x  1)  vector  of  constant  parameters  and  Y  is  an  (n  x  p) 
matrix  which  is  a  function  of  joint  positions,  velocities  and  accelerations.  This 
property  of  linearity  in  the  dynamic  parameters  is  fundamental  for  deriving 
adaptive  control  laws,  among  which  the  technique  illustrated  below  is  one  of 
the  simplest. 

At  first,  a  control  scheme  which  can  be  derived  through  a  combined  com¬ 
puted  torque/inverse  dynamics  approach  is  illustrated.  The  computational 
model  is  assumed  to  coincide  with  the  dynamic  model. 

Consider  the  control  law 

u  =  B(q)qr  +  C{q,  q)qr  +  Fqr  +  g(q)  +  KDcr,  (8.91) 

with  K d  a  positive  definite  matrix.  The  choice 

qr  =  Qd  +  AQ  Qr  =  Qd  +  AQ »  (8-92) 

with  A  a  positive  definite  (usually  diagonal)  matrix,  allows  the  nonlinear  com¬ 
pensation  and  decoupling  terms  to  be  expressed  as  a  function  of  the  desired 
velocity  and  acceleration,  corrected  by  the  current  state  (q  and  q)  of  the  ma¬ 
nipulator.  In  fact,  notice  that  the  term  qr  =  qd  +  Aq  weighs  the  contribution 
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that  depends  on  velocity,  not  only  on  the  basis  of  the  desired  velocity  but  also 
on  the  basis  of  the  position  tracking  error.  A  similar  argument  also  holds  for 
the  acceleration  contribution,  where  a  term  depending  on  the  velocity  tracking 
error  is  considered  besides  the  desired  acceleration. 

The  term  K ]jcr  is  equivalent  to  a  PD  action  on  the  error  if  er  is  taken  as 

cr  =  qr-q  =  q  +  Aq.  (8.93) 

Substituting  (8.91)  into  (8.90)  and  accounting  for  (8.93)  yields 

B(q)&  +  C(q,q)er  +  Fcr  +  Kdct  =  0.  (8.94) 

Consider  the  Lyapunov  function  candidate 

V{<r,q)  =  ^(TTB(q)a-+  ^qT  Mq  >  0  Ver,q^0,  (8.95) 

where  M  is  an  (?i  x  n)  symmetric  positive  definite  matrix;  the  introduction 
of  the  second  term  in  (8.95)  is  necessary  to  obtain  a  Lyapunov  function  of  the 
entire  system  state  which  vanishes  for  q  =  0  and  q  =  0.  The  time  derivative 
of  V  along  the  trajectories  of  system  (8.94)  is 

V  =  <JT  B(q)&  +  -crT  B(q)cr  +  qT  Mq 

=  —crT(F  +  Kd)ct  +  qTMq,  (8.96) 

where  the  skew-symmetry  propertyv  of  the  matrix  N  =  B  —  2 C  has  been 
exploited.  In  view  of  the  expression  of  er  in  (8.93),  with  diagonal  A  and  Kd, 
it  is  convenient  to  choose  M  =  2 AKjj\  this  leads  to 

V  =  —crTFcr  —  q  Kc/q  —  q^AKnAq.  (8.97) 

This  expression  shows  that  the  time  derivative  is  negative  definite  since  it 
vanishes  only  if  q  =  0  and  q  =  0;  thus,  it  follows  that  the  origin  of  the  state 
space  [qT  crT]T  =  0  is  globally  asymptotically  stable.  It  is  worth  noticing 
that,  unlike  the  robust  control  case,  the  error  trajectory  tends  to  the  subspace 
er  =  0  without  the  need  of  a  high-frequency  control. 

On  the  basis  of  this  notable  result,  the  control  law  can  be  made  adaptive 
with  respect  to  the  vector  of  parameters  tt. 

Suppose  that  the  computational  model  has  the  same  structure  as  that  of 
the  manipulator  dynamic  model,  but  its  parameters  are  not  known  exactly. 
The  control  law  (8.91)  is  then  modified  into 


u  =  B(q)qr  +  C(q.  q)qr  +  Fqr  +  g  +  KDa 

=  Y {q,  q,  Qr:  Qr)n  +  KD(T, 


(8.98) 
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where  tv  represents  the  available  estimate  on  the  parameters  and,  accordingly, 
B,  C ,  F,  g  denote  the  estimated  terms  in  the  dynamic  model.  Substituting 
control  (8.98)  into  (8.90)  gives 

B(q)&  +  C(q,  q)a  +  Fc r  +  KDa  =  -B(q)qr  -  C(q,  q)qr  -  Fqr  -  g(q) 

= -Y(q,q,qr,qr)TV,  (8.99) 

where  the  property  of  linearity  in  the  error  parameter  vector 

n  =  tv  —  tv  (8.100) 

has  been  conveniently  exploited.  In  view  of  (8.63),  the  modelling  error  is 
characterized  by 

B  =  B  B  C  =  C-C  F  =  F-F  g  =  g-g.  (8.101) 

It  is  worth  remarking  that,  in  view  of  position  (8.92),  the  matrix  Y  does  not 
depend  on  the  actual  joint  accelerations  but  only  on  their  desired  values;  this 
avoids  problems  due  to  direct  measurement  of  acceleration. 

At  this  point,  modify  the  Lyapunov  function  candidate  in  (8.95)  into  the 
form 

V (a,  q,  tv)  =  ^aTB(q)a  +  q1  AKDq  +  ^-^K^tv  >  0  Ver,  q,  tv  ^  0, 

(8.102) 

which  features  an  additional  term  accounting  for  the  parameter  error  (8.100), 
with  Kir  symmetric  positive  definite.  The  time  derivative  of  V  along  the 
trajectories  of  system  (8.99)  is 

V  =  —<yT Fcr  -  qT K Dq  -  q1  AKDAq  +  tvT  (K^tt  -  YT(q ,  q,  qr,  qr)a) . 

(8.103) 

If  the  estimate  of  the  parameter  vector  is  updated  as  in  the  adaptive  law 

tv  =  K~1YT(q,q,qr,qr)a,  (8.104) 

the  expression  in  (8.103)  becomes 

V  =  —aT Fa  q  K £>q  —  q1  AK nAq 

since  tv  =  tv  tv  is  constant. 

By  an  argument  similar  to  above,  it  is  not  difficult  to  show  that  the  tra¬ 
jectories  of  the  manipulator  described  by  the  model 

B(q)q  +  C{q,  q)q  +  Fq  +  g{q)  =  u, 

under  the  control  law 

u  =  Y(q,q,qr,qr)n  +  KD(q  +  Aq) 
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Fig.  8.26.  Block  scheme  of  joint  space  adaptive  control 


and  the  parameter  adaptive  law 

7?  =  K~1YT(q,  q ,  qr,  qr)(q  +  Aq), 

globally  asymptotically  converge  to  cr  =  0  and  q  =  0,  which  implies  conver¬ 
gence  to  zero  of  q,  q,  and  boundedness  of  tv.  The  equation  in  (8.99)  shows 
that  asymptotically  it  is 


Y{q,q,qr,qr)(n  ~  *)  =  0-  (8.105) 

This  equation  does  not  imply  that  7?  tends  to  7r;  indeed,  convergence  of  param¬ 
eters  to  their  true  values  depends  on  the  structure  of  the  matrix  Y (q,  q,  qr ,  qr) 
and  then  on  the  desired  and  actual  trajectories.  Nonetheless,  the  followed  ap¬ 
proach  is  aimed  at  solving  a  direct  adaptive  control  problem,  i.e.,  finding  a 
control  law  that  ensures  limited  tracking  errors,  and  not  at  determining  the 
actual  parameters  of  the  system  (as  in  an  indirect  adaptive  control  problem). 
The  resulting  block  scheme  is  illustrated  in  Fig.  8.26.  To  summarize,  the  above 
control  law  is  formed  by  three  different  contributions: 

•  The  term  Yri  describes  a  control  action  of  inverse  dynamics  type  which 
ensures  an  approximate  compensation  of  nonlinear  effects  and  joint  decou¬ 
pling. 

•  The  term  K^cr  introduces  a  stabilizing  linear  control  action  of  PD  type 
on  the  tracking  error. 

•  The  vector  of  parameter  estimates  7r  is  updated  by  an  adaptive  law  of 
gradient  type  so  as  to  ensure  asymptotic  compensation  of  the  terms  in  the 
manipulator  dynamic  model;  the  matrix  Kn  determines  the  convergence 
rate  of  parameters  to  their  asymptotic  values. 

Notice  that,  with  cr  «  0,  the  control  law  (8.98)  is  equivalent  to  a  pure 
inverse  dynamics  compensation  of  the  computed  torque  type  on  the  basis  of 
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desired  velocities  and  accelerations;  this  is  made  possible  by  the  fact  that 

YlZ  «  Y 7T. 

The  control  law  with  parameter  adaptation  requires  the  availability  of  a 
complete  computational  model  and  it  does  not  feature  any  action  aimed  at 
reducing  the  effects  of  external  disturbances.  Therefore,  a  performance  degra¬ 
dation  is  expected  whenever  unmodelled  dynamic  effects,  e.g.,  when  a  reduced 
computational  model  is  used,  or  external  disturbances  occur.  In  both  cases, 
the  effects  induced  on  the  output  variables  are  attributed  by  the  controller  to 
parameter  estimate  mismatching;  as  a  consequence,  the  control  law  attempts 
to  counteract  those  effects  by  acting  on  quantities  that  did  not  provoke  them 
originally. 

On  the  other  hand,  robust  control  techniques  provide  a  natural  rejection 
to  external  disturbances,  although  they  are  sensitive  to  unmodelled  dynamics; 
this  rejection  is  provided  by  a  high-frequency  commuted  control  action  that 
constrains  the  error  trajectories  to  stay  on  the  sliding  subspace.  The  resulting 
inputs  to  the  mechanical  structure  may  be  unacceptable.  This  inconvenience 
is  in  general  not  observed  with  the  adoption  of  adaptive  control  techniques 
whose  action  has  a  naturally  smooth  time  behaviour. 


8.6  Operational  Space  Control 

In  all  the  above  control  schemes,  it  was  always  assumed  that  the  desired  tra¬ 
jectory  is  available  in  terms  of  the  time  sequence  of  the  values  of  joint  position, 
velocity  and  acceleration.  Accordingly,  the  error  for  the  control  schemes  was 
expressed  in  the  joint  space. 

As  often  pointed  out,  motion  specifications  are  usually  assigned  in  the  op¬ 
erational  space,  and  then  an  inverse  kinematics  algorithm  has  to  be  utilized  to 
transform  operational  space  references  into  the  corresponding  joint  space  ref¬ 
erences.  The  process  of  kinematic  inversion  has  an  increasing  computational 
load  when,  besides  inversion  of  direct  kinematics,  inversion  of  first-order  and 
second-order  differential  kinematics  is  also  required  to  transform  the  desired 
time  history  of  end-effector  position,  velocity  and  acceleration  into  the  corre¬ 
sponding  quantities  at  the  joint  level.  It  is  for  this  reason  that  current  indus¬ 
trial  robot  control  systems  compute  the  joint  positions  through  kinematics 
inversion,  and  then  perform  a  numerical  differentiation  to  compute  velocities 
and  accelerations. 

A  different  approach  consists  of  considering  control  schemes  developed 
directly  in  the  operational  space.  If  the  motion  is  specified  in  terms  of  opera¬ 
tional  space  variables,  the  measured  joint  space  variables  can  be  transformed 
into  the  corresponding  operational  space  variables  through  direct  kinematics 
relations.  Comparing  the  desired  input  with  the  reconstructed  variables  allows 
the  design  of  feedback  control  loops  where  trajectory  inversion  is  replaced  with 
a  suitable  coordinate  transformation  embedded  in  the  feedback  loop. 
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Fig.  8.27.  Block  scheme  of  Jacobian  inverse  control 


All  operational  space  control  schemes  present  considerable  computational 
requirements,  in  view  of  the  necessity  to  perform  a  number  of  computations 
in  the  feedback  loop  which  are  somewhat  representative  of  inverse  kinematics 
functions.  With  reference  to  a  numerical  implementation,  the  presence  of  a 
computationally  demanding  load  requires  sampling  times  that  may  lead  to 
degrading  the  performance  of  the  overall  control  system. 

In  the  face  of  the  above  limitations,  it  is  worth  presenting  operational 
space  control  schemes,  whose  utilization  becomes  necessary  when  the  prob¬ 
lem  of  controlling  interaction  between  the  manipulator  and  the  environment 
is  of  concern.  In  fact,  joint  space  control  schemes  suffice  only  for  motion  con¬ 
trol  in  the  free  space.  When  the  manipulator’s  end-effector  is  constrained  by 
the  environment,  e.g.,  in  the  case  of  end-effector  in  contact  with  an  elastic 
environment,  it  is  necessary  to  control  both  positions  and  contact  forces  and 
it  is  convenient  to  refer  to  operational  space  control  schemes.  Hence,  below 
some  solutions  are  presented;  these  are  worked  out  for  motion  control,  but 
they  constitute  the  premise  for  the  force/position  control  strategies  that  will 
be  illustrated  in  the  next  chapter. 

8.6.1  General  Schemes 

As  pointed  out  above,  operational  space  control  schemes  are  based  on  a  direct 
comparison  of  the  inputs,  specifying  operational  space  trajectories,  with  the 
measurements  of  the  corresponding  manipulator  outputs.  It  follows  that  the 
control  system  should  incorporate  some  actions  that  allow  the  transformation 
from  the  operational  space,  in  which  the  error  is  specified,  to  the  joint  space, 
in  which  control  generalized  forces  are  developed. 

A  possible  control  scheme  that  can  be  devised  is  the  so-called  Jacobian 
inverse  control  (Fig.  8.27).  In  this  scheme,  the  end-effector  pose  in  the  op¬ 
erational  space  xe  is  compared  with  the  corresponding  desired  quantity  x d, 
and  then  an  operational  space  deviation  Ax  can  be  computed.  Assumed  that 
this  deviation  is  sufficiently  small  for  a  good  control  system,  Ax  can  be  trans¬ 
formed  into  a  corresponding  joint  space  deviation  Aq  through  the  inverse  of 
the  manipulator  Jacobian.  Then,  the  control  input  generalized  forces  can  be 
computed  on  the  basis  of  this  deviation  through  a  suitable  feedback  matrix 
gain.  The  result  is  a  presumable  reduction  of  Aq  and  in  turn  of  Ax.  In  other 
words,  the  Jacobian  inverse  control  leads  to  an  overall  system  that  intuitively 
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Fig.  8.28.  Block  scheme  of  Jacobian  transpose  control 


behaves  like  a  mechanical  system  with  a  generalized  n-dimensional  spring  in 
the  joint  space,  whose  constant  stiffness  is  determined  by  the  feedback  matrix 
gain.  The  role  of  such  system  is  to  take  the  deviation  Aq  to  zero.  If  the  matrix 
gain  is  diagonal,  the  generalized  spring  corresponds  to  n  independent  elastic 
elements,  one  for  each  joint. 

A  conceptually  analogous  scheme  is  the  so-called  Jacobian  transpose  con¬ 
trol  (Fig.  8.28).  In  this  case,  the  operational  space  error  is  treated  first  through 
a  matrix  gain.  The  output  of  this  block  can  then  be  considered  as  the  elas¬ 
tic  force  generated  by  a  generalized  spring  whose  function  in  the  operational 
space  is  that  to  reduce  or  to  cancel  the  position  deviation  Ax.  In  other  words, 
the  resulting  force  drives  the  end-effector  along  a  direction  so  as  to  reduce  Ax. 
This  operational  space  force  has  then  to  be  transformed  into  the  joint  space 
generalized  forces,  through  the  transpose  of  the  Jacobian,  so  as  to  realize  the 
described  behaviour. 

Both  Jacobian  inverse  and  transpose  control  schemes  have  been  derived 
in  an  intuitive  fashion.  Hence,  there  is  no  guarantee  that  such  schemes  are 
effective  in  terms  of  stability  and  trajectory  tracking  accuracy.  These  problems 
can  be  faced  by  presenting  two  mathematical  solutions  below,  which  will  be 
shown  to  be  substantially  equivalent  to  the  above  schemes. 


8.6.2  PD  Control  with  Gravity  Compensation 

By  analogy  with  joint  space  stability  analysis,  given  a  constant  end-effector 
pose  Xd,  it  is  desired  to  find  the  control  structure  so  that  the  operational  space 
error 

x  =  Xd  —  xe  (8.106) 

tends  asymptotically  to  zero.  Choose  the  following  positive  definite  quadratic 
form  as  a  Lyapunov  function  candidate: 

V(q,x)  =  ^qTB(q)q+^xTKPx>0  Vq,5^0,  (8.107) 

with  Kp  a  symmetric  positive  definite  matrix.  Differentiating  (8.107)  with 
respect  to  time  gives 

V  =  q1  B(q)q  +  ^ q 1  B(q)q  +  x?  KPx. 
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Fig.  8.29.  Block  scheme  of  operational  space  PD  control  with  gravity  compensation 


Since  Xd  =  0,  in  view  of  (3.62)  it  is 

x  =  -JA{q)q 

and  then 

V  =  q1  B{q)q  +  ^ q 1  B(q)q  -  q1  JA(q)KPx.  (8.108) 

By  recalling  the  expression  of  the  joint  space  manipulator  dynamic  model 
in  (8.7)  and  the  property  in  (7.49),  the  expression  in  (8.108)  becomes 

V  =  -qTFq  +  qT(u-  g(q)  -  JTA(q)KPx).  (8.109) 

This  equation  suggests  the  structure  of  the  controller;  in  fact,  by  choosing 
the  control  law 


u  =  g(q)  +  JTA(q)KPx  -  JTA{q)K DJ A(q)q  (8.110) 

with  Kp >  positive  definite,  (8.109)  becomes 

V=~qTFq-qTJTA(q)KDJA(q)q.  (8.111) 

As  can  be  seen  from  Fig.  8.29,  the  resulting  block  scheme  reveals  an  anal¬ 
ogy  with  the  scheme  of  Fig.  8.28.  Control  law  (8.110)  performs  a  nonlin¬ 
ear  compensating  action  of  joint  space  gravitational  forces  and  an  operational 
space  linear  PD  control  action.  The  last  term  has  been  introduced  to  enhance 
system  damping;  in  particular,  if  measurement  of  x  is  deduced  from  that  of 
q,  one  can  simply  choose  the  derivative  term  as  —Kpq. 

The  expression  in  (8.111)  shows  that,  for  any  system  trajectory,  the  Lya¬ 
punov  function  decreases  as  long  as  q  /  0.  The  system  then  reaches  an  equi¬ 
librium  posture.  By  a  stability  argument  similar  to  that  in  the  joint  space 
(see  (8.52)— (8.54) )  this  posture  is  determined  by 

JA(q)KPx  =  0. 


(8.112) 
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Fig.  8.30.  Block  scheme  of  operational  space  inverse  dynamics  control 


From  (8.112)  it  can  be  recognized  that,  under  the  assumption  of  full-rank 
Jacobian,  it  is 

x  =  xd  -  xe  =  0, 

i.e.,  the  sought  result. 

If  measurements  of  xe  and  xe  are  made  directly  in  the  operational  space, 
k(q)  and  J  a{q)  in  the  scheme  of  Fig.  8.45  are  just  indicative  of  direct  kine¬ 
matics  functions;  it  is,  however,  necessary  to  measure  q  to  update  both 
and  g(q )  on-line.  If  measurements  of  operational  space  quantities  are  indirect, 
the  controller  has  to  compute  the  direct  kinematics  functions,  too. 

8.6.3  Inverse  Dynamics  Control 

Consider  now  the  problem  of  tracking  an  operational  space  trajectory.  Recall 
the  manipulator  dynamic  model  in  the  form  (8.55) 

B(q)q  +  n{q,q)  =  u , 

where  n  is  given  by  (8.56).  As  in  (8.57),  the  choice  of  the  inverse  dynamics 
linearizing  control 

u  =  B{q)y  +  n(q,q) 
leads  to  the  system  of  double  integrators 

q  =  y.  (8.113) 

The  new  control  input  y  is  to  be  designed  so  as  to  yield  tracking  of  a  trajectory 
specified  by  Xd(t).  To  this  end,  the  second-order  differential  equation  in  the 
form  (3.98) 

xe  =  J  A{q)q  +  J  A{q,q)q 
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suggests,  for  a  nonredundant  manipulator,  the  choice  of  the  control  law  — 
formally  analogous  to  (3.102)  — 

V  =  J^1{q)(xd  +  Kdx  +  KPx  -  jA(q,q)q)  (8.114) 

with  K p  and  K p>  positive  definite  (diagonal)  matrices.  In  fact,  substitut¬ 
ing  (8.114)  into  (8.113)  gives 


x  +  K  px  +  K  px  =  0  (8.115) 

which  describes  the  operational  space  error  dynamics,  with  Kp  and  Kp> 
determining  the  error  convergence  rate  to  zero.  The  resulting  inverse  dynamics 
control  scheme  is  reported  in  Fig.  8.30,  which  confirms  the  anticipated  analogy 
with  the  scheme  of  Fig.  8.27.  Again  in  this  case,  besides  xe  and  xe,  q  and  q  are 
also  to  be  measured.  If  measurements  of  xe  and  xe  are  indirect,  the  controller 
must  compute  the  direct  kinematics  functions  k(q)  and  J A(q)  on-line. 

A  critical  analysis  of  the  schemes  in  Figs.  8.29,  8.30  reveals  that  the  design 
of  an  operational  space  controller  always  requires  computation  of  manipulator 
Jacobian.  As  a  consequence,  controlling  a  manipulator  in  the  operational  space 
is  in  general  more  complex  than  controlling  it  in  the  joint  space.  In  fact,  the 
presence  of  singularities  and/or  redundancy  influences  the  Jacobian,  and  the 
induced  effects  are  somewhat  difficult  to  handle  with  an  operational  space 
controller.  For  instance,  if  a  singularity  occurs  for  the  scheme  of  Fig.  8.29  and 
the  error  enters  the  null  space  of  the  Jacobian,  the  manipulator  gets  stuck 
at  a  different  configuration  from  the  desired  one.  This  problem  is  even  more 
critical  for  the  scheme  of  Fig.  8.30  which  would  require  the  computation  of  a 
DLS  inverse  of  the  Jacobian.  Yet,  for  a  redundant  manipulator,  a  joint  space 
control  scheme  is  naturally  transparent  to  this  situation,  since  redundancy 
has  already  been  solved  by  inverse  kinematics,  whereas  an  operational  space 
control  scheme  should  incorporate  a  redundancy  handling  technique  inside 
the  feedback  loop. 

As  a  final  remark,  the  above  operational  space  control  schemes  have  been 
derived  with  reference  to  a  minimal  description  of  orientation  in  terms  of 
Euler  angles.  It  is  understood  that,  similar  to  what  is  presented  in  Sect.  3.7.3 
for  inverse  kinematics  algorithms,  it  is  possible  to  adopt  different  definitions 
of  orientation  error,  e.g.,  based  on  the  angle  and  axis  or  the  unit  quaternion. 
The  advantage  is  the  use  of  the  geometric  Jacobian  in  lieu  of  the  analytical 
Jacobian.  The  price  to  pay,  however,  is  a  more  complex  analysis  of  the  stability 
and  convergence  characteristics  of  the  closed-loop  system.  Even  the  inverse 
dynamics  control  scheme  will  not  lead  to  a  homogeneous  error  equation,  and 
a  Lyapunov  argument  should  be  invoked  to  ascertain  its  stability. 
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8.7  Comparison  Among  Various  Control  Schemes 

In  order  to  make  a  comparison  between  the  various  control  schemes  presented, 
consider  the  two-link  planar  arm  with  the  same  data  of  Example  7.2: 

a±  =  a2  =  lm  t\=  l2  =  0.5m  =  m^2  :  50kg  I(±  =  I(2  =  10kg-m2 

kri  =  kr2  =  100  rnmi  =  mm2  =  5  kg  Iml  =  Im2  =  0.01  kg-m2. 

The  arm  is  assumed  to  be  driven  by  two  equal  actuators  with  the  following 
data: 

Fmi  =  Fm2  =  0.01N-m-s/rad  Ra  i  =  Ra2  =  10  ohm 

kti  =  kt2  =  2N-m/A  kv\  =  kv2  =  2V-s/rad; 

it  can  be  verified  that  Fmi  <C  kVikti/Rai  for  i  =  1,2. 

The  desired  tip  trajectories  have  a  typical  trapezoidal  velocity  profile,  and 
thus  it  is  anticipated  that  sharp  torque  variations  will  be  induced.  The  tip  path 
is  a  motion  of  1.6  m  along  the  horizontal  axis,  as  in  the  path  of  Example  7.2.  In 
the  first  case  ( fast  trajectory),  the  acceleration  time  is  0.6s  and  the  maximum 
velocity  is  lm/s.  In  the  second  case  ( slow  trajectory),  the  acceleration  time  is 
0.6  s  and  the  maximum  velocity  is  0.25  m/s.  The  motion  of  the  controlled  arm 
was  simulated  on  a  computer,  by  adopting  a  discrete-time  implementation  of 
the  controller  with  a  sampling  time  of  1  ms. 

The  following  control  schemes  in  the  joint  space  and  in  the  operational 
space  have  been  utilized;  an  (analytical)  inverse  kinematics  solution  has 
been  implemented  to  generate  the  reference  inputs  to  the  joint  space  con¬ 
trol  schemes: 

A.  Independent  joint  control  with  position  and  velocity  feedback  (Fig.  5.11) 
with  the  following  data  for  each  joint  servo: 

KP  =  5  Ky  =  10  kpp  =  kpy  =  1, 

corresponding  to  cun  =  5  rad/s  and  (  =  0.5. 

B.  Independent  joint  control  with  position,  velocity  and  acceleration  feedback 
(Fig.  8.9)  with  the  following  data  for  each  joint  servo: 

Kp  =  5  Ky  =  10  K  a  =  2  kpp  =  kpy  =  kpA  =  1> 

corresponding  to  u>n  =  5  rad/s,  (  =  0.5,  Xp  =  100.  To  reconstruct  accel¬ 
eration,  a  first-order  filter  has  been  utilized  (Fig.  8.11)  characterized  by 
u>3f  =  100  rad/s. 

C.  As  in  scheme  A  with  the  addition  of  a  decentralized  feedforward  action 
(Fig.  8.13). 

D.  As  in  scheme  B  with  the  addition  of  a  decentralized  feedforward  action 
(Fig.  8.14). 
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E.  Joint  space  computed  torque  control  (Fig.  8.19)  with  feedforward  com¬ 
pensation  of  the  diagonal  terms  of  the  inertia  matrix  and  of  gravitational 
terms,  and  decentralized  feedback  controllers  as  in  scheme  A. 

F.  Joint  space  PD  control  with  gravity  compensation  (Fig.  8.20),  modified 
by  the  addition  of  a  feedforward  velocity  term  K  p>qd,  with  the  following 
data: 

KP  =  3750 1 2  Kd  =  750I2. 

G.  Joint  space  inverse  dynamics  control  (Fig.  8.22)  with  the  following  data: 

KP  =  25 I2  Kd  =  5  I2. 

H.  Joint  space  robust  control  (Fig.  8.23),  under  the  assumption  of  constant 
inertia  ( B  =  B )  and  compensation  of  friction  and  gravity  (ra  =  Fvq  +  g), 
with  the  following  data: 

KP  =  25  J2  Kd  =  5J2  P  =  I2  p  =  70  e  =  0.004. 

I.  As  in  case  H  with  e  =  0.01. 

J.  Joint  space  adaptive  control  (Fig.  8.26)  with  a  parameterization  of  the 
arm  dynamic  model  (7.82)  as  in  (7.83),  (7.84).  The  initial  estimate  of  the 
vector  7?  is  computed  on  the  basis  of  the  nominal  parameters.  The  arm 
is  supposed  to  carry  a  load  which  causes  the  following  variations  on  the 
second  link  parameters: 

Z\?n2  =  10kg  Am2£c2  =  H  kg-m  AI2  =  12.12  kg-m2. 

This  information  is  obviously  utilized  only  to  update  the  simulated  arm 
model.  Further,  the  following  data  are  set: 

A  =  5I2  Kd  =  750I2  K„  =  0.01I8. 

K.  Operational  space  PD  control  with  gravity  compensation  (Fig.  8.29),  mod¬ 
ified  by  the  addition  of  a  feedforward  velocity  term  K pxd,  with  the  fol¬ 
lowing  data: 

KP  =  16250 J2  Kd  =  3250 I2. 

L.  Operational  space  inverse  dynamics  control  (Fig.  8.30)  with  the  following 
data: 

KP  =  25 12  Kd  =  5 12. 

It  is  worth  remarking  that  the  adopted  model  of  the  dynamic  system  of  arm 
with  drives  is  that  described  by  (8.7).  In  the  decentralized  control  schemes  A 
E,  the  joints  have  been  voltage-controlled  as  in  the  block  scheme  of  Fig.  8.3, 
with  unit  amplifier  gains  (Gv  =  I).  On  the  other  hand,  in  the  centralized 
control  schemes  F-L,  the  joints  have  been  current-controlled  as  in  the  block 
scheme  of  Fig.  8.4,  with  unit  amplifier  gains  (G,  =  I). 

Regarding  the  parameters  of  the  various  controllers,  these  have  been  cho¬ 
sen  in  such  a  way  as  to  allow  a  significant  comparison  of  the  performance  of 
each  scheme  in  response  to  congruent  control  actions.  In  particular,  it  can  be 
observed  that: 
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Fig.  8.31.  Time  history  of  the  joint  positions  and  torques  and  of  the  tip  position 
errors  for  the  fast  trajectory  with  control  scheme  A 
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Fig.  8.32.  Time  history  of  the  joint  torques  and  of  the  norm  of  tip  position  error 
for  the  fast  trajectory;  left:  with  control  scheme  C,  right :  with  control  scheme  D 
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Fig.  8.33.  Time  history  of  the  joint  torques  and  of  the  norm  of  tip  position  error 
for  the  fast  trajectory  with  control  scheme  E 
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Fig.  8.34.  Time  history  of  the  joint  positions  and  torques  and  of  the  norm  of  tip 
position  error  for  the  fast  trajectory  with  control  scheme  F 
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Fig.  8.35.  Time  history  of  the  joint  torques  and  of  the  norm  of  tip  position  error 
for  the  fast  trajectory  with  control  scheme  G 
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Fig.  8.36.  Time  history  of  the  joint  torques  and  of  the  norm  of  tip  position  error 
for  the  fast  trajectory;  left:  with  control  scheme  H,  right:  with  control  scheme  I 


•  The  dynamic  behaviour  of  the  joints  is  the  same  for  schemes  A  E. 

•  The  gains  of  the  PD  actions  in  schemes  G,  H,  I  and  L  have  been  chosen 
so  as  to  obtain  the  same  natural  frequency  and  damping  ratios  as  those  of 
schemes  A  E. 

The  results  obtained  with  the  various  control  schemes  are  illustrated  in 
Figs.  8.31-8.39  for  the  fast  trajectory  and  in  Figs.  8.40-8.48  for  the  slow 
trajectory,  respectively.  In  the  case  of  two  quantities  represented  in  the  same 
plot  notice  that: 

•  For  the  joint  trajectories,  the  dashed  line  indicates  the  reference  trajectory 
obtained  from  the  tip  trajectory  via  inverse  kinematics,  while  the  solid  line 
indicates  the  actual  trajectory  followed  by  the  arm. 

•  For  the  joint  torques,  the  solid  line  refers  to  Joint  1  while  the  dashed  line 
refers  to  Joint  2. 

•  For  the  tip  position  error,  the  solid  line  indicates  the  error  component  along 
the  horizontal  axis  while  the  dashed  line  indicates  the  error  component 
along  the  vertical  axis. 

Finally,  the  representation  scales  have  been  made  as  uniform  as  possible 
in  order  to  allow  a  more  direct  comparison  of  the  results. 

Regarding  performance  of  the  various  control  schemes  for  the  fast  trajec¬ 
tory,  the  obtained  results  lead  to  the  following  considerations. 


354 


8  Motion  Control 


pos  error  norm  parameter  error  norm 


Fig.  8.37.  Time  history  of  the  norm  of  tip  position  error  and  of  the  norm  of  pa¬ 
rameter  error  vector  for  the  fast  trajectory  with  control  scheme  J 
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Fig.  8.38.  Time  history  of  the  joint  torques  and  of  the  norm  of  tip  position  error 
for  the  fast  trajectory  with  control  scheme  K 
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Fig.  8.39.  Time  history  of  the  joint  torques  and  of  the  norm  of  tip  position  error 
for  the  fast  trajectory  with  control  scheme  L 


Deviation  of  the  actual  joint  trajectories  from  the  desired  ones  shows  that 
tracking  performance  of  scheme  A  is  quite  poor  (Fig.  8.31).  It  should  be 
noticed,  however,  that  the  largest  contribution  to  the  error  is  caused  by  a 
time  lag  of  the  actual  trajectory  behind  the  desired  one,  while  the  distance 
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Fig.  8.40.  Time  history  of  the  joint  positions  and  torques  and  of  the  tip  position 
errors  for  the  slow  trajectory  with  control  scheme  A 
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Fig.  8.41.  Time  history  of  the  joint  torques  and  of  the  norm  of  tip  position  error 
for  the  slow  trajectory;  left:  with  control  scheme  C,  right :  with  control  scheme  D 
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Fig.  8.42.  Time  history  of  the  joint  torques  and  of  the  norm  of  tip  position  error 
for  the  slow  trajectory  with  control  scheme  E 
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Fig.  8.43.  Time  history  of  the  joint  positions  and  torques  and  of  the  norm  of  tip 
position  error  for  the  slow  trajectory  with  control  scheme  F 
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Fig.  8.44.  Time  history  of  the  joint  torques  and  of  the  norm  of  tip  position  error 
for  the  slow  trajectory  with  control  scheme  G 
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Fig.  8.45.  Time  history  of  the  joint  torques  and  of  the  norm  of  tip  position  error 
for  the  slow  trajectory;  left:  with  control  scheme  H,  right :  with  control  scheme  I 


of  the  tip  from  the  geometric  path  is  quite  contained.  Similar  results  were 
obtained  with  scheme  B,  and  then  they  have  not  been  reported. 

With  schemes  C  and  D,  an  appreciable  tracking  accuracy  improvement  is 
observed  (Fig.  8.32),  with  better  performance  for  the  second  scheme,  thanks 
to  the  outer  acceleration  feedback  loop  that  allows  a  disturbance  rejection 
factor  twice  as  much  as  for  the  first  scheme.  Notice  that  the  feedforward 
action  yields  a  set  of  torques  which  are  closer  to  the  nominal  ones  required  to 
execute  the  desired  trajectory;  the  torque  time  history  has  a  discontinuity  in 
correspondence  of  the  acceleration  and  deceleration  fronts. 

The  tracking  error  is  further  decreased  with  scheme  E  (Fig.  8.33),  by  virtue 
of  the  additional  nonlinear  feedforward  compensation. 

Scheme  F  guarantees  stable  convergence  to  the  final  arm  posture  with  a 
tracking  performance  which  is  better  than  that  of  schemes  A  and  B,  thanks  to 
the  presence  of  a  velocity  feedforward  action,  but  worse  than  that  of  schemes 
C  E,  in  view  of  lack  of  an  acceleration  feedforward  action  (Fig.  8.34). 

As  would  be  logical  to  expect,  the  best  results  are  observed  with  scheme  G 
for  which  the  tracking  error  is  practically  zero,  and  it  is  mainly  due  to  numer¬ 
ical  discretization  of  the  controller  (Fig.  8.35). 

It  is  then  worth  comparing  the  performance  of  schemes  H  and  I  (Fig.  8.36). 
In  fact,  the  choice  of  a  small  threshold  value  for  e  (scheme  H)  induces  high- 
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Fig.  8.46.  Time  history  of  the  norm  of  tip  position  error  and  of  the  norm  of  pa¬ 
rameter  error  vector  for  the  slow  trajectory  with  control  scheme  J 
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Fig.  8.47.  Time  history  of  the  joint  torques  and  of  the  norm  of  tip  position  error 
for  the  slow  trajectory  with  control  scheme  K 
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Fig.  8.48.  Time  history  of  the  joint  torques  and  of  the  norm  of  tip  position  error 
for  the  slow  trajectory  with  control  scheme  L 

frequency  components  in  Joint  1  torque  (see  the  thick  portions  of  the  torque 
plot)  at  the  advantage  of  a  very  limited  tracking  error.  As  the  threshold  value  is 
increased  (scheme  I) ,  the  torque  assumes  a  smoother  behaviour  at  the  expense 
of  a  doubled  norm  of  tracking  error,  though. 
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For  scheme  J,  a  lower  tracking  error  than  that  of  scheme  F  is  observed, 
thanks  to  the  effectiveness  of  the  adaptive  action  on  the  parameters  of  the 
dynamic  model.  Nonetheless,  the  parameters  do  not  converge  to  their  nominal 
values,  as  confirmed  by  the  time  history  of  the  norm  of  the  parameter  error 
vector  that  reaches  a  non-null  steady-state  value  (Fig.  8.37). 

Finally,  the  performance  of  schemes  K  and  L  is  substantially  comparable 
to  that  of  corresponding  schemes  F  and  G  (Figs.  8.38  and  8.39). 

Performance  of  the  various  control  schemes  for  the  slow  trajectory  is  glob¬ 
ally  better  than  that  for  the  fast  trajectory.  Such  improvement  is  particularly 
evident  for  the  decentralized  control  schemes  (Figs.  8.40-8.42),  whereas  the 
tracking  error  reduction  for  the  centralized  control  schemes  is  less  dramatic 
(Figs.  8.43-8.48),  in  view  of  the  small  order  of  magnitude  of  the  errors  already 
obtained  for  the  fast  trajectory.  In  any  case,  as  regards  performance  of  each 
single  scheme,  it  is  possible  to  make  a  number  of  remarks  analogous  to  those 
previously  made. 
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Problems 

8.1.  With  reference  to  the  block  scheme  with  position  feedback  in  Fig.  5.10, 
find  the  transfer  functions  of  the  forward  path,  the  return  path,  and  the 
closed- loop  system. 

8.2.  With  reference  to  the  block  scheme  with  position  and  velocity  feedback 
in  Fig.  5.11,  find  the  transfer  functions  of  the  forward  path,  the  return  path, 
and  the  closed-loop  system. 

8.3.  With  reference  to  the  block  scheme  with  position,  velocity  and  accelera¬ 
tion  feedback  in  Fig.  8.9,  find  the  transfer  functions  of  the  forward  path,  the 
return  path,  and  the  closed-loop  system. 

8.4.  For  a  single  joint  drive  system  with  the  data:  I  =  6kg-m2,  Ra  =  0.3  ohm, 
kt  —  0.5N-m/A,  kv  =  0.5  V-s/rad,  Fm  =  0.001  N-m-s/rad,  find  the  parameters 
of  the  controller  with  position  feedback  (unit  transducer  constant)  that  yield  a 
closed-loop  response  with  damping  ratio  £  >  0.4.  Discuss  disturbance  rejection 
properties. 

8.5.  For  the  drive  system  of  Problem  8.4,  find  the  parameters  of  the  controller 
with  position  and  velocity  feedback  (unit  transducer  constants)  that  yield 
a  closed-loop  response  with  damping  ratio  £  >  0.4  and  natural  frequency 
u)n  =  20  rad/s.  Discuss  disturbance  rejection  properties. 

8.6.  For  the  drive  system  of  Problem  8.4,  find  the  parameters  of  the  controller 
with  position,  velocity  and  acceleration  feedback  (unit  transducer  constants) 
that  yield  a  closed-loop  response  with  damping  ratio  £  >  0.4,  natural  fre¬ 
quency  u)n  =  20  rad/s  and  disturbance  rejection  factor  Xr  =  400.  Also,  design 
a  first-order  filter  that  allows  acceleration  measurement  reconstruction. 

8.7.  Verify  that  the  control  schemes  in  Figs.  8.12,  8.13,  8.14  correspond  to 
realizing  (8.42),  (8.43),  (8.44),  respectively. 

8.8.  Verify  that  the  standard  regulation  schemes  in  Figs.  8.15,  8.16,  8.17  are 
equivalent  to  the  schemes  in  Figs.  8.12,  8.13,  8.14,  respectively. 

8.9.  Prove  inequality  (8.76). 

8.10.  For  the  two-link  planar  arm  with  the  same  data  as  in  Sect.  8.7,  design  a 

joint  control  of  PD  type  with  gravity  compensation.  By  means  of  a  computer 
simulation,  verify  stability  for  the  following  postures  q  =  [7r/4  — 7t/2]t  and 

q  =  [  —7 r  — 37t/4  ]t',  respectively.  Implement  the  control  in  discrete-time  with 
a  sampling  time  of  1  ms. 

8.11.  For  the  two-link  planar  arm  with  the  same  data  as  in  Sect.  8.7,  under 
the  assumption  of  a  concentrated  tip  payload  of  mass  tul  =  10  kg,  design 
an  independent  joint  control  with  feedforward  computed  torque.  Perform  a 
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computer  simulation  of  the  motion  of  the  controlled  arm  along  the  joint  space 
rectilinear  path  from  qi  =  [ 0  7t/4]t  to  =  [tt/2  k/2]t  with  atrapezoidal 

velocity  profile  and  a  trajectory  duration  tf  =  Is.  Implement  the  control  in 
discrete-time  with  a  sampling  time  of  1ms. 

8.12.  For  the  two-link  planar  arm  of  Problem  8.11,  design  an  inverse  dynamics 
joint  control.  Perform  a  computer  simulation  of  the  motion  of  the  controlled 
arm  along  the  trajectory  specified  in  Problem  8.11.  Implement  the  control  in 
discrete-time  with  a  sampling  time  of  1ms. 

8.13.  For  the  two-link  planar  arm  of  Problem  8.11,  design  a  robust  joint  con¬ 
trol.  Perform  a  computer  simulation  of  the  motion  of  the  controlled  arm  along 
the  trajectory  specified  in  Problem  8.11.  Implement  the  control  in  discrete¬ 
time  with  a  sampling  time  of  1  ms. 

8.14.  For  the  two-link  planar  arm  of  Problem  8.11,  design  an  adaptive  joint 
control,  on  the  basis  of  a  suitable  parameterization  of  the  arm  dynamic  model. 
Perform  a  computer  simulation  of  the  motion  of  the  controlled  arm  along  the 
trajectory  specified  in  Problem  8.11.  Implement  the  control  in  discrete-time 
with  a  sampling  time  of  1  ms. 

8.15.  For  the  two-link  planar  of  Problem  8.11,  design  a  PD  control  in  the 

operational  space  with  gravity  compensation.  By  means  of  a  computer  sim¬ 
ulation,  verify  stability  for  the  following  postures  p  =  [0.5  0.5  ]T  and 

p  =  [0.6  — 0.2  ]r,  respectively.  Implement  the  control  in  discrete-time  with 

a  sampling  time  of  1  ms. 

8.16.  For  the  two-link  planar  arm  of  Problem  8.11,  design  an  inverse  dynamics 

control  in  the  operational  space.  Perform  a  computer  simulation  of  the  motion 
of  the  controlled  arm  along  the  operational  space  rectlinear  path  from  p(0)  = 
[0.7  0.2  ]T  to  p(l)  =  [0.1  —  0.6]t  with  a  trapezoidal  velocity  profile  and  a 

trajectory  duration  tf  =  Is.  Implement  the  control  in  discrete-time  with  a 
sampling  time  of  1ms. 
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Force  Control 


One  of  the  fundamental  requirements  for  the  success  of  a  manipulation  task 
is  the  capacity  to  handle  interaction  between  manipulator  and  environment. 
The  quantity  that  describes  the  state  of  interaction  more  effectively  is  the 
contact  force  at  the  manipulator’s  end-effector.  High  values  of  contact  force 
are  generally  undesirable  since  they  may  stress  both  the  manipulator  and  the 
manipulated  object.  In  this  chapter,  performance  of  operational  space  motion 
control  schemes  is  studied  first,  during  the  interaction  of  a  manipulator  with 
the  environment.  The  concepts  of  mechanical  compliance  and  impedance  are 
introduced,  with  special  regard  to  the  problem  of  integrating  contact  force 
measurements  into  the  control  strategy.  Then,  force  control  schemes  are  pre¬ 
sented  which  are  obtained  from  motion  control  schemes  suitably  modified  by 
the  closure  of  an  outer  force  regulation  feedback  loop.  For  the  planning  of  con¬ 
trol  actions  to  perform  an  interaction  task,  natural  constraints  set  by  the  task 
geometry  and  artificial  constraints  set  by  the  control  strategy  are  established; 
the  constraints  are  referred  to  a  suitable  constraint  frame.  The  formulation  is 
conveniently  exploited  to  derive  hybrid  force/motion  control  schemes. 


9.1  Manipulator  Interaction  with  Environment 

Control  of  interaction  between  a  robot  manipulator  and  the  environment  is 
crucial  for  successful  execution  of  a  number  of  practical  tasks  where  the  robot’s 
end-effector  has  to  manipulate  an  object  or  perform  some  operation  on  a  sur¬ 
face.  Typical  examples  include  polishing,  deburring,  machining  or  assembly. 
A  complete  classification  of  possible  robot  tasks  is  practically  infeasible  in 
view  of  the  large  variety  of  cases  that  may  occur,  nor  would  such  a  classifica¬ 
tion  be  really  useful  to  find  a  general  strategy  to  interaction  control  with  the 
environment. 

During  the  interaction,  the  environment  sets  constraints  on  the  geometric 
paths  that  can  be  followed  by  the  end-effector.  This  situation  is  generally 
referred  to  as  constrained  motion.  In  such  a  case,  the  use  of  a  purely  motion 
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control  strategy  for  controlling  interaction  is  a  candidate  to  fail,  as  explained 
below. 

Successful  execution  of  an  interaction  task  with  the  environment  by  using 
motion  control  could  be  obtained  only  if  the  task  were  accurately  planned. 
This  would,  in  turn,  require  an  accurate  model  of  both  the  robot  manipulator 
(kinematics  and  dynamics)  and  the  environment  (geometry  and  mechanical 
features).  Manipulator  modelling  can  be  achieved  with  enough  precision,  but 
a  detailed  description  of  the  environment  is  difficult  to  obtain. 

To  understand  the  importance  of  task  planning  accuracy,  it  is  sufficient  to 
observe  that  to  perform  a  mechanical  part  mating  with  a  positional  approach, 
the  relative  positioning  of  the  parts  should  be  guaranteed  with  an  accuracy 
of  an  order  of  magnitude  greater  than  part  mechanical  tolerance.  Once  the 
absolute  position  of  one  part  is  exactly  known,  the  manipulator  should  guide 
the  motion  of  the  other  with  the  same  accuracy. 

In  practice,  the  planning  errors  may  give  rise  to  a  contact  force  causing  a 
deviation  of  the  end-effector  from  the  desired  trajectory.  On  the  other  hand, 
the  control  system  reacts  to  reduce  such  deviation.  This  ultimately  leads  to  a 
build-up  of  the  contact  force  until  saturation  of  the  joint  actuators  is  reached 
or  breakage  of  the  parts  in  contact  occurs. 

The  higher  the  environment  stiffness  and  position  control  accuracy,  the 
more  likely  a  situation  like  the  one  just  described  can  occur.  This  drawback 
can  be  overcome  if  compliant  behaviour  is  ensured  during  the  interaction. 

From  the  above  discussion  it  should  be  clear  that  the  contact  force  is 
the  quantity  describing  the  state  of  interaction  in  the  most  complete  fashion; 
to  this  end,  the  availability  of  force  measurements  is  expected  to  provide 
enhanced  performance  for  controlling  interaction. 

Interaction  control  strategies  can  be  grouped  in  two  categories;  those  per¬ 
forming  indirect  force  control  and  those  performing  direct  force  control.  The 
main  difference  between  the  two  categories  is  that  the  former  achieve  force 
control  via  motion  control,  without  explicit  closure  of  a  force  feedback  loop; 
the  latter,  instead,  offer  the  possibility  of  controlling  the  contact  force  to  a 
desired  value,  thanks  to  the  closure  of  a  force  feedback  loop.  To  the  first  cate¬ 
gory  belong  compliance  control  and  impedance  control  which  are  treated  next. 
Then,  force  control  and  hybrid  force/motion  control  schemes  will  follow. 


9.2  Compliance  Control 

For  a  detailed  analysis  of  interaction  between  the  manipulator  and  environ¬ 
ment  it  is  worth  considering  the  behaviour  of  the  system  under  a  position 
control  scheme  when  contact  forces  arise.  Since  these  are  naturally  described 
in  the  operational  space,  it  is  convenient  to  refer  to  operational  space  control 
schemes. 
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Consider  the  manipulator  dynamic  model  (8.7).  In  view  of  (7.42),  the 
model  can  be  written  as 

B(q)q  +  C(q,  q)q  +  Fq  +  g(q)  =  u-  JT(q)he  (9.1) 

where  he  is  the  vector  of  contact  forces  exerted  by  the  manipulator’s  end- 
effector  on  the  environment.1 

It  is  reasonable  to  predict  that,  in  the  case  he  ^  0,  the  control  scheme 
based  on  (8.110)  no  longer  ensures  that  the  end-effector  reaches  its  desired 
pose  Xd .  In  fact,  by  recalling  that  x  =  Xd  —  xe,  where  xe  denotes  the  end- 
effector  pose,  at  the  equilibrium  it  is 

JTA(q)KPx  =  JT(q)he.  (9.2) 

On  the  assumption  of  a  full-rank  Jacobian,  one  has 

X  =  K^T^xJK  =  K~PxhA  (9.3) 

where  hA  is  the  vector  of  equivalent  forces  that  can  be  defined  according 
to  (7.128).  The  expression  in  (9.3)  shows  that  at  the  equilibrium  the  manip¬ 
ulator,  under  a  pose  control  action,  behaves  like  a  generalized  spring  in  the 
operational  space  with  compliance  Kf,1  in  respect  of  the  equivalent  force  hA. 
By  recalling  the  expression  of  the  transformation  matrix  T A  in  (3.65)  and  as¬ 
suming  matrix  Kp  to  be  diagonal,  it  can  be  recognized  that  linear  compliance 
(due  to  force  components)  is  independent  of  the  configuration,  whereas  tor¬ 
sional  compliance  (due  to  moment  components)  does  depend  on  the  current 
end-effector  orientation  through  the  matrix  T. 

On  the  other  hand,  if  he  £  A f{JT),  one  has  x  =  0  with  he  ^  0,  namely 
contact  forces  are  completely  balanced  by  the  manipulator  mechanical  struc¬ 
ture;  for  instance,  the  anthropomorphic  manipulator  at  a  shoulder  singularity 
in  Fig.  3.13  does  not  react  to  any  force  orthogonal  to  the  plane  of  the  structure. 

Equation  (9.3)  can  be  rewritten  in  the  form 

hA  =  KPx  (9.4) 

where  Kp  represents  a  stiffness  matrix  as  regards  the  vector  of  the  equivalent 
forces  hA.  It  is  worth  observing  that  the  compliant  (or  stiff)  behaviour  of  the 
manipulator  is  achieved  by  virtue  of  the  control.  This  behaviour  is  termed 
active  compliance  whereas  the  term  passive  compliance  denotes  mechanical 
systems  with  a  prevalent  dynamics  of  elastic  type. 

For  a  better  understanding  of  the  interaction  between  manipulator  and 
environment,  it  is  necessary  to  analyze  further  the  concept  of  passive  compli¬ 
ance. 


1  In  this  chapter  the  term  force,  in  general,  is  referred  to  a  (6  x  1)  vector  of  force 
and  moment,  unless  otherwise  specified. 
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9.2.1  Passive  Compliance 


Consider  two  elastically  coupled  rigid  bodies  R  and  S  and  two  reference 
frames,  each  attached  to  one  of  the  two  bodies  so  that  at  equilibrium,  in 
the  absence  of  interaction  forces  and  moments,  the  two  frames  coincide.  Let 
dxrs  denote  an  elementary  displacement  from  the  equilibrium  of  frame  s  with 
respect  to  frame  r,  defined  as 


dx 


r,s 


dor^a 
w  i  sdt 


=  vrsdt 


(9.5) 


where  vTtS  =  vs  —  vr  is  the  vector  of  linear  and  angular  velocity  of  frame 
s  with  respect  to  frame  r,  do,.}S  =  os  —  or  is  the  vector  corresponding  to 
the  translation  of  the  origin  os  of  frame  s  with  respect  to  the  origin  or  of 
frame  r  and  u>r^sdt,  with  u>r>s  =  us  —  u)r,  represents  the  vector  of  small 
rotations  of  frame  s  about  the  axes  of  frame  r  as  in  (3.106).  This  elementary 
displacement  is  assumed  to  be  equivalently  referred  to  frame  r  or  s  because, 
at  the  equilibrium,  the  two  frames  coincide;  therefore,  the  reference  frame  was 
not  explicitly  denoted. 

To  the  displacement  dxrs,  coinciding  with  the  deformation  of  the  spring 
between  R  and  S,  it  corresponds  the  elastic  force 
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applied  by  body  S  on  the  spring  and  referred  equivalently  to  one  of  the  two 
reference  frames.  In  view  of  the  action-reaction  law,  the  force  applied  by  R 
has  the  expression  hr  =  hs  =  Kdxs^r,  being  dxSjr  =  —dxrjS. 

The  (6  x  6)  matrix  K  represents  a  stiffness  matrix,  which  is  symmetric 
and  positive  semi- definite.  The  (3  x  3)  matrices  Kf  and  are  known  as 
translational  stiffness  and  rotational  stiffness,  respectively.  The  (3  x  3)  matrix 
K,  is  known  as  coupling  stiffness.  An  analogous  decomposition  can  be  made 
for  the  compliance  matrix  C  in  the  mapping 


dx,.a  —  Chs. 


(9.7) 


In  the  real  elastic  systems,  matrix  Kc  is,  in  general,  non-symmetric.  How¬ 
ever,  there  are  special  devices,  such  as  the  RCC  ( Remote  Centre  of  Compli¬ 
ance),  where  Kc  can  be  symmetric  or  null.  These  are  elastically  compliant  me¬ 
chanical  devices,  suitably  designed  to  achieve  maximum  decoupling  between 
translation  and  rotation,  that  are  interposed  between  the  manipulator  last 
link  and  the  end-effector.  The  aim  is  that  of  introducing  a  passive  compliance 
of  desired  value  to  facilitate  the  execution  of  assembly  tasks.  For  instance,  in  a 
peg-in-hole  insertion  task,  the  gripper  is  provided  with  a  device  ensuring  high 
stiffness  along  the  insertion  direction  and  high  compliance  along  the  other 
directions.  Therefore,  in  the  presence  of  unavoidable  position  displacements 
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from  the  planned  insertion  trajectory,  contact  forces  and  moments  arise  which 
modify  the  peg  position  so  as  to  facilitate  insertion. 

The  inconvenience  of  such  devices  is  their  low  versatility  to  different  op¬ 
erating  conditions  and  generic  interaction  tasks,  namely,  whenever  a  modifi¬ 
cation  of  the  compliant  mechanical  hardware  is  required. 

9.2.2  Active  Compliance 

The  aim  of  compliance  control  is  that  of  achieving  a  suitable  active  compliance 
that  can  be  easily  modified  acting  on  the  control  software  so  as  to  satisfy  the 
requirements  of  different  interaction  tasks. 

Notice  that  the  equilibrium  equations  in  (9.3)  and  (9.4)  show  that  the 
compliant  behaviour  with  respect  to  he  depends  on  the  actual  end-effector 
orientation,  also  for  elementary  displacements,  so  that,  in  practice,  the  selec¬ 
tion  of  stiffness  parameters  is  quite  difficult.  To  obtain  an  equilibrium  equation 
of  the  form  (9.6),  a  different  definition  of  error  in  the  operational  space  must 
be  considered. 

Let  Oe-xeyeze  and  Od-XdUdZd  denote  the  end-effector  frame  and  the  de¬ 
sired  frame  respectively.  The  corresponding  homogeneous  transformation  ma¬ 
trices  are 


Re 

Rd 

Od ' 

_0T  1  . 

Td  = 

oT 

1 . 

with  obvious  meaning  of  notation.  The  position  and  orientation  displacement 
of  the  end-effector  frame  with  respect  to  the  desired  frame  can  be  expressed 
in  terms  of  the  homogeneous  transformation  matrix 

od  ' 

J’e  ,  (9.8) 

The  new  error  vector  in  the 


(9.9) 

where  </>d  e  is  the  vector  of  Euler  angles  extracted  from  the  rotation  matrix 
.  The  minus  sign  in  (9.9)  depends  on  the  fact  that,  for  control  purposes,  the 
error  is  usually  defined  as  the  difference  between  the  desired  and  the  measured 
quantities. 

Computing  the  time  derivative  of  od  e  and  taking  into  account  (3.10), 
(3.11)  gives 

°d,e  =  RdiOe  -  Od)  -  S{ud)RTd\oe  -  Od).  (9.10) 

On  the  other  hand,  computing  the  time  derivative  of  e  and  taking  into 
account  (3.64),  yields  (see  Problem  9.1) 

4>d, e  =  T~\ct>d^e  =  T~\ct>d^RTd{ue  -  u >d). 


Td  =  (Td)-1Te  = 


Rde 

0T 


where  =  Rd  Re  and  od 


d,e  —  Rd(°e  °d)  • 

operational  space  can  be  defined  as 


x  =  — 


d,e’ 


0d,( 


(9.11) 
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Considering  that  the  desired  quantities  Od  and  Rd  are  constant,  vector  x  can 
be  expressed  in  the  form 

x  =  -Tl\<l>d>e)  R*  °r  ve  (9.12) 

being  ve  =  [oj  =  J(q)q  the  vector  of  linear  and  angular  velocity  of 

the  end-effector.  Therefore 

x  = -J  Ad(q,x)q,  (9.13) 

where  the  matrix 

JAd{q,x)  =  T-A\<t>d'e)  R£  °r  J(q )  (9.14) 

represents  the  analytical  Jacobian  corresponding  to  the  definition  (9.9)  of 
error  in  the  operational  space. 

The  PD  control  with  gravity  compensation  analogous  to  (8.110),  with  the 
definition  (9.9)  of  error  in  the  operational  space,  has  the  expression 

U  =  g{q)  +  JAd(q,x){KPx  -  KDJAd(q,x)q).  (9.15) 

Notice  that,  in  the  case  where  the  operational  space  is  defined  only  by  the 
position  components,  the  control  laws  (8.110)  and  (9.15)  differ  only  because 
the  position  error  (and  the  corresponding  derivative  term)  is  referred  to  the 
base  frame  in  (8.110),  while  it  is  referred  to  the  desired  frame  in  (9.15). 

In  the  absence  of  interaction,  the  asymptotic  stability  of  the  equilibrium 
pose  corresponding  to  x  =  0,  assuming  that  Kp  and  Kp>  are  symmetric  and 
positive  definite  matrices,  can  be  proven  using  the  Lyapunov  function 

V (q,  x)  =  X- qrB(q)q  +  ^xT K Px  >0  Vq,  x  ^  0, 

as  for  the  case  of  the  control  law  (8.110). 

In  the  presence  of  interaction  with  the  environment,  at  the  equilibrium  it 
is 

JTAd{q)KPx  =  JT{q)he ■  (9.16) 

hence,  assuming  a  full-rank  Jacobian,  the  following  equality  holds: 

hi  —  TAT((f)de)Kpx.  (9.17) 

The  above  equation,  to  be  compared  to  the  elastic  model  (9.6),  must  be 
rewritten  in  terms  of  elementary  displacements.  To  this  end,  taking  into  ac¬ 
count  (9.12)  and  (9.5),  it  is 

dx  =  x~  dt  =  T^{0){v^  — v^)dt  =  T^(0)dxed  (9.18) 

x=0 
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where  dxe A  is  the  elementary  displacement  of  the  desired  frame  with  respect 
to  the  end-effector  frame  about  the  equilibrium,  referred  to  any  of  the  two 
frames.  The  value  of  Ta( 0)  depends  on  the  particular  choice  of  Euler  angles; 
in  the  following,  angles  XYZ  are  adopted,  for  which  Ta{ 0)  =  I  (see  Prob¬ 
lem  3.13).  Therefore,  rewriting  (9.17)  in  terms  of  elementary  displacements 
gives 

he  =  KPdxed,  (9.19) 

which  is  formally  identical  to  (9.6),  where  vectors  are  assumed  to  be  referred 
to  the  desired  frame  or  to  the  end-effector  frame,  equivalently.  It  follows  that 
matrix  K p  has  the  meaning  of  an  active  stiffness  corresponding  to  a  gen¬ 
eralized  spring  acting  between  the  end-effector  frame  and  the  desired  frame. 
Equation  (9.19)  can  be  rewritten  in  the  equivalent  form 

dxeA  =  Kpxhe,  (9.20) 

showing  that  KJ,1  corresponds  to  an  active  compliance. 

The  selection  of  the  elements  of  matrix  Kp  must  be  made  taking  into  ac¬ 
count  geometry  and  mechanical  features  of  the  environment.  To  this  end,  as¬ 
sume  that  the  interaction  force  between  the  end-effector  and  the  environment 
derives  from  a  generalized  spring  acting  between  the  end-effector  frame  and 
a  reference  frame  Or—xryrzr  attached  to  the  environment  rest  position.  Con¬ 
sidering  an  elementary  displacement  dxr^e  between  the  two  reference  frames, 
the  corresponding  elastic  force  applied  by  the  end-effector  is 

he  =  Kdxr,e  (9-21) 

with  a  stiffness  matrix  K ,  where  vectors  can  be  referred,  equivalently,  to  the 
frame  attached  to  the  rest  position  of  the  environment  or  to  the  end-effector 
frame.  Typically,  the  stiffness  matrix  is  positive  semi-definite  because,  in  gen¬ 
eral,  the  interaction  forces  and  moments  belong  to  some  particular  directions, 
spanning  1Z(K). 

In  view  of  the  model  (9.21),  of  (9.19)  and  of  the  equality 


dx 


r,e 


dXr,d 


the  following  expression  of  the  contact  force  at  equilibrium  can  be  found: 

he  =  (J6  +  KKPl)  ~lKdxr.d.  (9.22) 

Substituting  this  expression  into  (9.20)  yields 

dxeA  =  Kp^Ie  +  KKp1)~1KdxrA,  (9.23) 

representing  the  pose  error  of  the  end-effector  at  the  equilibrium. 

Notice  that  vectors  in  (9.22)  and  (9.23)  can  be  referred,  equivalently,  to  the 
end-effector  frame,  to  the  desired  frame  or  to  the  frame  attached  to  the  envi¬ 
ronment  rest  position;  these  frames  coincide  at  equilibrium  (see  Problem  9.2). 


370  9  Force  Control 


Fig.  9.1.  Two-link  planar  arm  in  contact  with  an  elastically  compliant  plane 


The  analysis  of  (9.23)  shows  that  the  end-effector  pose  error  at  the  equi¬ 
librium  depends  on  the  environment  rest  position,  as  well  as  on  the  desired 
pose  imposed  by  the  control  system  of  the  manipulator.  The  interaction  of 
the  two  systems  (environment  and  manipulator)  is  influenced  by  the  mutual 
weight  of  the  respective  compliance  features. 

In  fact,  it  is  possible  to  modify  the  active  compliance  Kp 1  so  that  the 
manipulator  dominates  the  environment  and  vice  versa.  Such  a  dominance 
can  be  specified  with  reference  to  the  single  directions  of  the  operational 
space. 

For  a  given  environment  stiffness  K ,  according  to  the  prescribed  inter¬ 
action  task,  one  may  choose  large  values  of  the  elements  of  Kp  for  those 
directions  along  which  the  environment  has  to  comply  and  small  values  of 
the  elements  of  Kp  for  those  directions  along  which  the  manipulator  has  to 
comply.  As  a  consequence,  the  manipulator  pose  error  dxe  ^  tends  to  zero 
along  the  directions  where  the  environment  complies;  vice  versa,  along  the 
directions  where  the  manipulator  complies,  the  end-effector  pose  tends  to  the 
rest  pose  of  the  environment,  namely  dxe ^  ~  dxr^- 

Equation  (9.22)  gives  the  value  of  the  contact  force  at  the  equilibrium. 
This  expression  reveals  that,  along  the  directions  where  the  manipulator  stiff¬ 
ness  is  much  higher  than  the  environment  stiffness,  the  intensity  of  the  elastic 
force  mainly  depends  on  the  stiffness  of  the  environment  and  on  the  displace¬ 
ment  dxr  e  between  the  equilibrium  pose  of  the  end-effector  (which  practically 
coincides  with  the  desired  pose)  and  the  rest  pose  of  the  environment.  In  the 
dual  case  that  the  environment  stiffness  is  much  higher  than  the  manipulator 
stiffness,  the  intensity  of  the  elastic  force  mainly  depends  on  the  manipula¬ 
tor  stiffness  and  on  the  displacement  dxStCi  between  the  desired  pose  and  the 
equilibrium  pose  of  the  end-effector  (which  practically  coincides  with  the  rest 
pose  of  the  environment). 
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Example  9.1 

Consider  the  two-link  planar  arm  whose  tip  is  in  contact  with  a  purely  frictionless 
elastic  plane;  due  to  the  simple  geometry  of  the  problem,  involving  only  position 
variables,  all  the  quantities  can  be  conveniently  referred  to  the  base  frame.  Thus, 
control  law  (8.110)  will  be  adopted.  Let  or  =  [xr  0]T  denote  the  equilibrium 
position  of  the  plane,  which  is  assumed  to  be  orthogonal  to  axis  Xo  (Fig.  9.1).  The 
environment  stiffness  matrix  is 

K  =  Kf  =  diagjfc^O}, 

corresponding  to  the  absence  of  interaction  forces  along  the  vertical  direction  (fe  = 
[fx  0]T).  Let  oe  =  [ xe  ye]T  be  the  end-effector  position  and  Od  =  [xd  yd]T  be 
the  desired  position,  which  is  located  beyond  the  contact  plane.  The  proportional 
control  action  on  the  arm  is  characterized  by 

KP  =  diag{kPx,kpy}. 

The  equilibrium  equations  for  the  force  and  position  (9.22),  (9.23),  rewritten  with 
dxr,d  =  Od  —  Or  and  dxB,d  =  Od  —  oe,  give 


kpxkx  /  \ 

1  Xrl  X ) 

k'PxVCd  +  kxXr 

II 

<u 

kpx  +  kx 

oe  = 

kpx  kx 

0 

Vd 

With  reference  to  positioning  accuracy,  the  arm  tip  reaches  the  vertical  coordinate 
yd,  since  the  vertical  motion  direction  is  not  constrained.  As  for  the  horizontal 
direction,  the  presence  of  the  elastic  plane  imposes  that  the  arm  can  move  as  far  as 
it  reaches  the  coordinate  xe  <  Xd-  The  value  of  the  horizontal  contact  force  at  the 
equilibrium  is  related  to  the  difference  between  Xd  and  xr  by  an  equivalent  stiffness 
coefficient  which  is  given  by  the  parallel  composition  of  the  stiffness  coefficients 
of  the  two  interacting  systems.  Hence,  the  arm  stiffness  and  environment  stiffness 
influence  the  resulting  equilibrium  configuration.  In  the  case  when 

kpx/kx  1, 


it  is 

Xe~Xd  fx~kx(xd-Xr ) 

and  thus  the  arm  prevails  over  the  environment,  in  that  the  plane  complies  almost 
up  to  Xd  and  the  elastic  force  is  mainly  generated  by  the  environment  (passive 
compliance).  In  the  opposite  case 


kpx/kx  1, 


it  is 


Xe  ~  Xr  fx  ~  kpx(Xd  Xr ) 


and  thus  the  environment  prevails  over  the  arm  which  complies  up  to  the  equilibrium 
xr,  and  the  elastic  force  is  mainly  generated  by  the  arm  (active  compliance). 
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To  complete  the  analysis  of  manipulator  compliance  in  contact  with  envi¬ 
ronment,  it  is  worth  considering  the  effects  of  a  joint  space  position  control 
law.  With  reference  to  (8.51),  in  the  presence  of  end-effector  contact  forces, 
the  equilibrium  posture  is  determined  by 


KPq  =  JT(q)he 

(9.24) 

q  =  Kp1  JT(q)he. 

(9.25) 

On  the  assumption  of  small  displacements  from  the  equilibrium,  it  is  reason¬ 
able  to  compute  the  resulting  operational  space  displacement  as  dx  ss  J(q)dq, 
referred  to  the  base  frame.  Therefore,  in  view  of  (9.25)  it  is 

dx  =  J{q)Kp1JT{q)he ,  (9.26) 

corresponding  to  an  active  compliance  referred  to  the  base  frame.  Notice  that 
the  compliance  matrix  J(q)Kp1JT(q)  depends  on  the  manipulator  posture, 
both  for  the  force  and  moment  components.  Also  in  this  case,  the  occurrence 
of  manipulator  Jacobian  singularities  is  to  be  analyzed  apart. 


9.3  Impedance  Control 

It  is  now  desired  to  analyze  the  interaction  of  a  manipulator  with  the  envi¬ 
ronment  under  the  action  of  an  inverse  dynamics  control  in  the  operational 
space.  With  reference  to  model  (9.1),  consider  the  control  law  (8.57) 

u=  B{q)y  +  n(q,q), 

with  n  as  in  (8.56).  In  the  presence  of  end-effector  forces,  the  controlled  ma¬ 
nipulator  is  described  by 


q  =  y-B-\q)JT(q)he  (9.27) 

that  reveals  the  existence  of  a  nonlinear  coupling  term  due  to  contact  forces. 
Choose  y  in  a  way  conceptually  analogous  to  (8.114),  as 

y  =  JA1(q)M^1(Mdxd  +  KDx  +  KPx  -  MdjA(q,  q)q)  (9.28) 

where  Md  is  a  positive  definite  diagonal  matrix.  Substituting  (9.28)  into  (9.27) 
and  accounting  for  second-order  differential  kinematics  in  the  form  (3.98), 
yields 

Mdx  +  Kdx  +  KPx  =  MdBA1(q)hA,  (9.29) 

BA{q)  =  JAT(q)B(q)JA\q) 


where 
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is  the  inertia  matrix  of  the  manipulator  in  the  operational  space  as  in  (7.134); 
this  matrix  is  configuration-dependent  and  is  positive  definite  if  J  A  has  full 
rank. 

The  expression  in  (9.29)  establishes  a  relationship  through  a  generalized 
mechanical  impedance  between  the  vector  of  forces  M dB ~^Ha  and  the  vector 
of  displacements  x  in  the  operational  space.  This  impedance  can  be  attributed 
to  a  mechanical  system  characterized  by  a  mass  matrix  Md,  a  damping  matrix 
Kp,  and  a  stiffness  matrix  Kp,  which  can  be  used  to  specify  the  dynamic 
behaviour  along  the  operational  space  directions. 

The  presence  of  B makes  the  system  coupled.  If  it  is  wished  to  keep 
linearity  and  decoupling  during  interaction  with  the  environment,  it  is  then 
necessary  to  measure  the  contact  force;  this  can  be  achieved  by  means  of 
appropriate  force  sensors  which  are  usually  mounted  to  the  manipulator  wrist, 
as  discussed  in  Sect.  5.4.1.  Choosing 

u  =  B(q)y  +  n(q,q)  +  JT  (q)he  (9.30) 


with 

y  =  J^l(q)Mfl(Mdxd  +  KDx  +  KPx  -  MdJ  A(q,q)q  -  hA ),  (9.31) 

under  the  assumption  of  error-free  force  measurements,  yields 

Mdx  +  K  px  +  K  Px  =  hA.  (9.32) 

It  is  worth  noticing  that  the  addition  of  the  term  JThe  in  (9.30)  exactly 
compensates  the  contact  forces  and  then  it  renders  the  manipulator  infinitely 
stiff  as  regards  the  external  stress.  In  order  to  confer  a  compliant  behaviour 
to  the  manipulator,  the  term  —J^1M'f1hA  has  been  introduced  in  (9.31)  so 
that  the  manipulator  can  be  characterized  as  a  linear  impedance  with  regard 
to  the  equivalent  forces  h A,  as  shown  in  (9.32). 

The  behaviour  of  the  system  in  (9.32)  at  the  equilibrium  is  analogous  to 
that  described  by  (9.4);  nonetheless,  compared  to  a  control  with  active  com¬ 
pliance  specified  by  KJ,1,  the  equation  in  (9.32)  allows  a  complete  characteri¬ 
zation  of  system  dynamics  through  an  active  impedance  specified  by  matrices 
Md,  Kd,  KP.  Also  in  this  case,  it  is  not  difficult  to  recognize  that,  as  re¬ 
gards  he,  impedance  depends  on  the  current  end-effector  orientation  through 
the  matrix  T.  Therefore,  the  selection  of  the  impedance  parameters  becomes 
difficult;  moreover,  an  inadequate  behaviour  may  occur  in  the  neighbourhood 
of  representation  singularities. 

To  avoid  this  problem  it  is  sufficient  to  redesign  the  control  input  y  as  a 
function  of  the  operational  space  error  (9.9). 

Under  the  assumption  that  the  desired  frame  Od-xdydzd  is  time-varying, 
in  view  of  (9.10),  (9.11),  the  time  derivative  of  (9.9)  has  the  expression 

x  =  —J Ad{q,  x)q  +  b(x,  Rd,  odl  u)d), 


(9.33) 
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Fig.  9.2.  Block  scheme  of  impedance  control 


where  J  Ad  is  the  analytical  Jacobian  (9.14)  and  vector  b  is 
h(~  td  .  ,  x  r R%dd+s(wi)oddt  1 

b(x>  Rd,Od,UJd)  —  J1-1/I  \  d 

\*rd,e)^d 

Computing  the  time  derivative  of  (9.33)  yields 

x  =  -J Adq  -  JAdq  +  £>, 


(9.34) 


where,  for  simplicity,  the  dependence  of  the  functions  on  their  arguments  was 
omitted.  As  a  consequence,  using  (9.30)  with 

y  =  J~A]M-dl  (Kdx  +  KPx  -  MdJ  Adq  +  Mdb  -  h%  (9.35) 

yields  the  equation 

Mdx  +  K  Dx  +  K  Px  =  hg,  (9.36) 

where  all  the  vectors  are  referred  to  the  desired  frame.  This  equation  repre¬ 
sents  a  linear  impedance  as  regards  the  force  vector  faf,  independent  from  the 
manipulator  configuration. 

The  block  scheme  representing  impedance  control  is  reported  in  Fig.  9.2. 

Similar  to  active  and  passive  compliance,  the  concept  of  passive  impe¬ 
dance  can  be  introduced  if  the  interaction  force  he  is  generated  at  the  contact 
with  an  environment  of  proper  mass,  damping  and  stiffness.  In  this  case, 
the  system  of  manipulator  with  environment  can  be  regarded  as  a  mechanical 
system  constituted  by  the  parallel  of  the  two  impedances,  and  then  its  dynamic 
behaviour  is  conditioned  by  the  relative  weight  between  them. 


Example  9.2 

Consider  the  planar  arm  in  contact  with  an  elastically  compliant  plane  of  the  pre¬ 
vious  example.  Due  to  the  simple  geometry  of  the  problem,  involving  only  position 
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Fig.  9.3.  Equivalent  block  scheme  of  a  manipulator  in  contact  with  an  elastic 
environment  under  impedance  control 


variables,  all  the  quantities  can  be  conveniently  referred  to  the  base  frame.  Thus, 
the  impedance  control  law  with  force  measurement  (9.30),  (9.31)  will  be  adopted. 
Moreover,  Xd  =  Od,  x  =  Od  —  oe,  Ha  =  fe  and 

M d  =  Amg{mdx,mdy}  K D  =  diag{ kDx ,  kDy }  KP  =  diag{ kPx ,  kPy } . 

The  block  scheme  of  the  manipulator  in  contact  with  an  elastic  environment 
under  impedance  control  is  represented  in  Fig.  9.3,  where  xe  =  oe  and  xr  =  or. 

If  Xd  is  constant,  the  dynamics  of  the  manipulator  and  environment  system 
along  the  two  directions  of  the  operational  space  is  described  by 

mdxXe  +  kDxXe  +  (kPx  +  kx)xe  =  kxXr  +  kPxXd 
rridyVe  +  koyi/e  +  kPyye  =  kPyyd- 


Along  the  vertical  direction,  one  has  an  unconstrained  motion  whose  time  be¬ 
haviour  is  determined  by  the  following  natural  frequency  and  damping  factor: 


_  c  - 

Ony  —  \  ^y  — 


'  mdykPy 


while,  along  the  horizontal  direction,  the  behaviour  of  the  contact  force  fx  =  kx{xe  — 
xr)  is  determined  by 


Below,  the  dynamic  behaviour  of  the  system  is  analyzed  for  two  different  values 
of  environment  compliance:  kx  =  103N/m  and  kx  =  104N/m.  The  rest  position  of 
the  environment  is  xr  =  1.  The  actual  arm  is  the  same  as  in  Example  7.2.  Apply 
an  impedance  control  with  force  measurement  of  the  kind  (9.30),  (9.31),  and  PD 
control  actions  equivalent  to  those  chosen  in  the  simulations  of  Sect.  8.7,  namely 


ixidx  —  rixidy  —  100  kPx  —  kPy  —  500  kPx  —  kPy  —  2500. 
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tip  y-pos  tip  x-force 


Fig.  9.4.  Time  history  of  the  tip  position  along  vertical  direction  and  of  the  contact 
force  along  horizontal  direction  with  impedance  control  scheme  for  environments  of 
different  compliance 


For  these  values  it  is 

oJny  =  5  rad/s  C,y  =  0.5. 

Then,  for  the  more  compliant  environment  it  is 

tOnx  «  5.9  rad/s  «  0.42 

whereas  for  the  less  compliant  environment  it  is 

ujnx  «  11.2  rad/s  C,x  «  0.22. 

Let  the  arm  tip  be  in  contact  with  the  environment  at  position  xe  =  [  1  0]T;  it  is 

desired  to  take  it  to  position  Xd  =  [1.1  0.1  ]T. 

The  results  in  Fig.  9.4  show  that  motion  dynamics  along  the  vertical  direction 
is  the  same  in  the  two  cases.  As  regards  the  contact  force  along  the  horizontal  direc¬ 
tion,  for  the  more  compliant  environment  ( dashed  line )  a  well-damped  behaviour  is 
obtained,  whereas  for  the  less  compliant  environment  (solid  line)  the  resulting  be¬ 
haviour  is  less  damped.  Further,  at  the  equilibrium,  in  the  first  case  a  displacement 
of  7.14cm  with  a  contact  force  of  71. 4N,  whereas  in  the  second  case  a  displacement 
of  2  cm  with  a  contact  force  of  200  N  are  observed. 


The  selection  of  good  impedance  parameters,  so  as  to  achieve  a  satisfactory 
behaviour  during  the  interaction,  is  not  an  easy  task.  Example  9.2  showed  that 
the  closed-loop  dynamics  along  the  free  motion  directions  is  different  from  the 
closed-loop  dynamics  along  the  constrained  directions.  In  this  latter  case,  the 
dynamic  behaviour  depends  on  the  stiffness  characteristics  of  the  environment. 
The  execution  of  a  complex  task,  involving  different  types  of  interaction,  may 
require  different  values  of  impedance  parameters. 

Notice  that  impedance  control,  in  the  absence  of  interaction  or  along  the 
directions  of  free  motion,  is  equivalent  to  an  inverse  dynamics  position  control. 
Therefore,  for  the  selection  of  the  impedance  parameters,  one  also  has  to  take 
into  account  the  need  to  ensure  high  values  to  the  rejection  factor  of  the 
disturbances  due  to  model  uncertainties  and  to  the  approximations  into  the 
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Fig.  9.5.  Block  scheme  of  admittance  control 


inverse  dynamics  computation.  Such  a  factor  increases  proportionally  to  the 
gain  matrix  Kp.  Hence  the  closed-loop  behaviour  is  the  more  degraded  by 
disturbances,  the  more  compliant  the  impedance  control  is  made  (by  choosing 
low  values  for  the  elements  of  K p)  to  keep  interaction  forces  limited. 

A  possible  solution  may  be  that  of  separating  the  motion  control  problem 
from  the  impedance  control  problem  according  to  the  control  scheme  repre¬ 
sented  in  Fig.  9.5.  The  scheme  is  based  on  the  concept  of  compliant  frame, 
which  is  a  suitable  reference  frame  describing  the  ideal  behaviour  of  the  end- 
effector  under  impedance  control.  This  frame  is  specified  by  the  position  of 
the  origin  o(,  the  rotation  matrix  Rt,  as  well  as  by  the  liner  and  angular 
velocities  and  accelerations.  These  quantities  can  be  computed  by  integrating 
the  impedance  equations  in  the  form 

Mtz  +  K  Dtz  +  KPtz  =  hde,  (9.37) 

starting  from  the  measurements  of  the  force  vector  he,  where  Mt,  Kp>t ,  K pt 
are  the  parameters  of  a  mechanical  impedance.  In  the  above  equation,  vector 
z  represents  the  operational  space  error  between  the  desired  frame  and  the 
compliant  frame,  as  defined  in  (9.9),  using  subscript  t  in  place  of  subscript  e. 

The  kinematic  variables  of  the  compliant  frame  are  then  input  to  the 
motion  control  of  inverse  dynamics  type,  computed  according  to  Eqs.  (9.28), 
(9.30).  In  this  way,  the  gains  of  the  motion  control  law  (9.28)  can  be  designed 
so  as  to  guarantee  a  high  value  of  the  disturbance  rejection  factor.  On  the 
other  hand,  the  gains  of  the  impedance  control  law  (9.37)  can  be  set  so  as  to 
guarantee  satisfactory  behaviour  during  the  interaction  with  the  environment. 
Stability  of  the  overall  system  can  be  ensured  provided  that  the  equivalent 
bandwidth  of  the  motion  control  loop  is  larger  than  the  equivalent  bandwidth 
of  the  impedance  control  loop. 

The  above  control  scheme  is  also  known  as  admittance  control  because 
Equation  (9.37)  corresponds  to  a  mechanical  admittance  being  used  by  the 
controller  to  generate  the  motion  variables  (outputs)  from  the  force  measure¬ 
ments  (inputs).  On  the  other  hand,  the  control  defined  by  Eqs.  (9.31)  or  (9.35) 
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and  (9.30)  can  be  interpreted  as  a  system  producing  equivalent  end-effector 
forces  (outputs)  from  the  measurements  of  the  motion  variables  (inputs),  thus 
corresponding  to  a  mechanical  impedance. 


9.4  Force  Control 

The  above  schemes  implement  an  indirect  force  control,  because  the  interac¬ 
tion  force  can  be  indirectly  controlled  by  acting  on  the  desired  pose  of  the 
end-effector  assigned  to  the  motion  control  system.  Interaction  between  ma¬ 
nipulator  and  environment  is  anyhow  directly  influenced  by  compliance  of  the 
environment  and  by  either  compliance  or  impedance  of  the  manipulator. 

If  it  is  desired  to  control  accurately  the  contact  force,  it  is  necessary  to 
devise  control  schemes  that  allow  the  desired  interaction  force  to  be  directly 
specified.  The  development  of  a  direct  force  control  system,  in  analogy  to  a 
motion  control  system,  would  require  the  adoption  of  a  stabilizing  PD  control 
action  on  the  force  error  besides  the  usual  nonlinear  compensation  actions. 
Force  measurements  may  be  corrupted  by  noise,  and  then  a  derivative  action 
may  not  be  implemented  in  practice.  The  stabilizing  action  is  to  be  provided 
by  suitable  damping  of  velocity  terms.  As  a  consequence,  a  force  control  sys¬ 
tem  typically  features  a  control  law  based  not  only  on  force  measurements  but 
also  on  velocity  measurements,  and  eventually  position  measurements  too. 

The  realization  of  a  force  control  scheme  can  be  entrusted  to  the  closure 
of  an  outer  force  regulation  feedback  loop  generating  the  control  input  for  the 
motion  control  scheme  the  manipulator  is  usually  endowed  with.  Therefore, 
force  control  schemes  are  presented  below,  which  are  based  on  the  use  of  an 
inverse  dynamics  position  control.  The  effectiveness  of  a  such  control  scheme 
depends  on  the  particular  interaction  cases  and,  in  general,  on  the  contact 
geometry.  To  this  end,  notice  that  a  force  control  strategy  is  meaningful  only 
for  those  directions  of  the  operational  space  along  which  interaction  forces 
between  manipulator  and  environment  may  arise. 

Below,  force  control  schemes  based  on  the  adoption  of  motion  control  laws 
of  inverse  dynamics  type  are  presented,  assuming  that  the  operational  space 
is  defined  only  by  position  variables.  Therefore,  the  end-effector  pose  can  be 
specified  by  the  operational  space  vector  xe  =  oe.  Moreover,  the  elastic  model 

fe  =  K(xe-xr)  (9.38) 

is  assumed  for  the  environment,  obtained  from  (9.21)  with  the  assumption  that 
only  forces  arise  at  the  contact.  In  (9.38),  consider  xr  =  or  and  assume  that 
the  axes  of  the  frame  attached  to  the  environment  rest  position  are  parallel 
to  the  axes  of  the  base  frame.  The  above  assumptions  allow  some  important 
features  of  force  control  to  be  evidenced. 


9.4  Force  Control  379 


Fig.  9.6.  Block  scheme  of  force  control  with  inner  position  loop 


9.4.1  Force  Control  with  Inner  Position  Loop 

With  reference  to  the  inverse  dynamics  law  with  force  measurement  (9.30), 
choose  in  place  of  (9.31),  the  control 

y  =  J~1{q)Mj1(-KDxe  +  KP(xF  -  xe )  -  MdJ(q,q)q)  (9.39) 

where  sp  is  a  suitable  reference  to  be  related  to  a  force  error.  Notice  that 
the  control  law  (9.39)  does  not  foresee  the  adoption  of  compensating  actions 
relative  to  xF  and  xF.  Moreover,  since  the  operational  space  is  defined  only 
by  position  variables,  the  analytical  Jacobian  coincides  with  the  geometric 
Jacobian  and  thus  Ja(q)  =  •/(</). 

Substituting  (9.30),  (9.39)  into  (9.1),  leads,  after  similar  algebraic  manip¬ 
ulation  as  above,  to  the  system  described  by 

Mdxe  +  Kp)Xe  +  Kpxe  =  Kpxp,  (9.40) 

which  shows  how  (9.30)  and  (9.39)  perform  a  position  control  taking  xe  to 
xp  with  a  dynamics  specified  by  the  choice  of  matrices  Mdl  Kp>,  Kp. 

Let  fd  denote  the  desired  constant  force  reference;  the  relation  between 
xp  and  the  force  error  can  be  expressed  as 

xF  =  CF{fd-fe),  (9.41) 

where  C p  is  a  diagonal  matrix,  with  the  meaning  of  compliance,  whose  ele¬ 
ments  give  the  control  actions  to  perform  along  the  operational  space  direc¬ 
tions  of  interest.  The  equations  in  (9.40),  (9.41)  reveal  that  force  control  is 
developed  on  the  basis  of  a  preexisting  position  control  loop. 

On  the  assumption  of  the  elastically  compliant  environment  described 
by  (9.38),  the  equation  in  (9.40)  with  (9.41)  becomes 

Mdxe  +  KDxe  +  Kp(I3  +  CFK)xe  =  KPCp{Kxr  +  fd).  (9.42) 

To  decide  about  the  kind  of  control  action  to  specify  with  Cp,  it  is  worth 
representing  (9.21),  (9.40),  (9.41)  in  terms  of  the  block  scheme  in  Fig.  9.6, 
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Fig.  9.7.  Block  scheme  of  force  control  with  inner  velocity  loop 


which  is  logically  derived  from  the  scheme  in  Fig.  9.3.  This  scheme  suggests 
that  if  C f  has  a  purely  proportional  control  action,  then  fe  cannot  reach  fd 
and  xr  influences  the  interaction  force  also  at  steady  state. 

If  C f  also  has  an  integral  control  action  on  the  force  components,  then  it  is 
possible  to  achieve  fe  =  fd  at  steady  state  and,  at  the  same  time,  to  reject  the 
effect  of  xr  on  f  e.  Hence,  a  convenient  choice  for  C  p  is  a  proportional-integral 
(PI)  action 

Cf  =  Kf  +  Ki  J  (•)  (k.  (9.43) 

The  dynamic  system  resulting  from  (9.42),  (9.43)  is  of  third  order,  and  then  it 
is  necessary  to  choose  adequately  the  matrices  Kp>,  Kp,  KF ,  Kj  in  respect 
of  the  characteristics  of  the  environment.  Since  the  values  of  environment 
stiffness  are  typically  high,  the  weight  of  the  proportional  and  integral  actions 
should  be  contained;  the  choice  of  Kp  and  Kj  influences  the  stability  margins 
and  the  bandwidth  of  the  system  under  force  control.  On  the  assumption  that 
a  stable  equilibrium  is  reached,  it  is  fe  =  fd  and  then 

Kxe  =  Kxr  +  fd.  (9.44) 


9.4.2  Force  Control  with  Inner  Velocity  Loop 

From  the  block  scheme  of  Fig.  9.6  it  can  be  observed  that,  if  the  position 
feedback  loop  is  opened,  xF  represents  a  velocity  reference,  and  then  an  in¬ 
tegration  relationship  exists  between  xF  and  xe.  This  leads  to  recognizing 
that,  in  this  case,  the  interaction  force  with  the  environment  coincides  with 
the  desired  value  at  steady  state,  even  with  a  proportional  force  controller 
CF.  In  fact,  choosing 

y  =  J~1(q)Md1(-KDxe  +  KPxF  -  MdJ(q ,  q)q),  (9.45) 

with  a  purely  proportional  control  structure  (C F  =  Kp)  on  the  force  error 
yields 


xF=KF(fd-fe ) 


(9.46) 
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Fig.  9.8.  Block  scheme  of  parallel  force/position  control 


and  then  system  dynamics  is  described  by 

Mdxe  +  KDxe  +  KPKFKxe  =  K  PK  F(Kxr  +  fd).  (9.47) 

The  relationship  between  position  and  contact  force  at  the  equilibrium  is  given 
by  (9.44).  The  corresponding  block  scheme  is  reported  in  Fig.  9.7.  It  is  worth 
emphasizing  that  control  design  is  simplified,  since  the  resulting  system  now  is 
of  second  order;2  it  should  be  noticed,  however,  that  the  absence  of  an  integral 
action  in  the  force  controller  does  not  ensure  reduction  of  the  effects  due  to 
unmodelled  dynamics. 


9.4.3  Parallel  Force/Position  Control 

The  force  control  schemes  presented  require  the  force  reference  to  be  consistent 
with  the  geometric  features  of  the  environment.  In  fact,  if  fd  has  components 
outside  7 Z(K),  both  (9.42)  (in  case  of  an  integral  action  in  C F)  and  (9.47) 
show  that,  along  the  corresponding  operational  space  directions,  the  compo¬ 
nents  of  fd  are  interpreted  as  velocity  references  which  cause  a  drift  of  the 
end-effector  position.  If  fd  is  correctly  planned  along  the  directions  outside 
1Z(K),  the  resulting  motion  governed  by  the  position  control  action  tends  to 
take  the  end-effector  position  to  zero  in  the  case  of  (9.42),  and  the  end-effector 
velocity  to  zero  in  the  case  of  (9.47).  Hence,  the  above  control  schemes  do  not 
allow  position  control  even  along  the  admissible  task  space  directions. 

If  it  is  desired  to  specify  a  desired  end-effector  pose  xd  as  in  pure  position 
control  schemes,  the  scheme  of  Fig.  9.6  can  be  modified  by  adding  the  reference 
xd  to  the  input  where  positions  are  summed.  This  corresponds  to  choosing 

y  =  J~1(q)Mj1  [—KDxe  +  KP(x  +  xF)  -  MdjA(q,q)q )  (9.48) 

where  x  =  xd  —  xe.  The  resulting  scheme  (Fig.  9.8)  is  termed  parallel 
force/position  control ,  in  view  of  the  presence  of  a  position  control  action 

2  The  matrices  Kp  and  KF  are  not  independent  and  one  may  refer  to  a  single 
matrix  K'F  =  KpKF. 


382  9  Force  Control 


tip  x-pos 


tip  x-force 


Fig.  9.9.  Time  history  of  the  tip  position  and  of  the  contact  force  along  horizontal 
direction  with  force  control  scheme  with  inner  position  loop  for  two  environments 
of  different  compliance 


Kpx  in  parallel  to  a  force  control  action  KpCp(fd  ~  fe)-  It  is  easy  to 
verify  that,  in  this  case,  the  equilibrium  position  satisfies  the  equation  (see 
Problem  9.4) 

xe  =  xd  +  CF(K(xr-xe)  +  fd).  (9.49) 

Therefore,  along  those  directions  outside  1Z{K)  where  motion  is  uncon¬ 
strained,  the  position  reference  xd  is  reached  by  xe.  Vice  versa,  along  those 
directions  in  1Z{K)  where  motion  is  constrained,  xd  is  treated  as  an  addi¬ 
tional  disturbance;  the  adoption  of  an  integral  action  in  Cp  as  for  the  scheme 
of  Fig.  9.6  ensures  that  the  force  reference  fd  is  reached  at  steady  state,  at 
the  expense  of  a  position  error  on  xe  depending  on  environment  compliance. 


Example  9.3 

Consider  again  the  planar  arm  in  contact  with  the  elastically  compliant  plane  of  the 
above  examples;  let  the  initial  contact  position  be  the  same  as  that  of  Example  9.2. 
Performance  of  the  various  force  control  schemes  is  analyzed;  as  in  Example  9.2, 
a  more  compliant  ( kx  =  103N/m)  and  a  less  compliant  (kx  =  104N/m)  environ¬ 
ment  are  considered.  The  position  control  actions  Md,  Kp,  Kp  are  chosen  as  in 
Example  9.2;  a  force  control  action  is  added  along  the  horizontal  direction,  namely 

CF  =  diagjcFx,  0}. 

The  reference  for  the  contact  force  is  chosen  as  f  d  =  [  10  0  ]T;  the  position  reference 

—  meaningful  only  for  the  parallel  control  —  is  taken  as  xd  =  [  1.015  0.1  ]T. 

With  regard  to  the  scheme  with  inner  position  loop  of  Fig.  9.6,  a  PI  control 
action  cfx  is  chosen  with  parameters: 

kFx  =  0.00064  kix  =  0.0016. 

This  confers  two  complex  poles  (—1.96,  ±j 5.74),  a  real  pole  (—1.09),  and  a  real  zero 
(—2.5)  to  the  overall  system,  for  the  more  compliant  environment. 
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Fig.  9.10.  Time  history  of  the  tip  position  and  of  the  contact  force  along  horizontal 
direction  with  force  control  scheme  with  inner  velocity  loop  for  two  environments  of 
different  compliance 


tip  x-pos  tip  x-force 


Fig.  9.11.  Time  history  of  tip  position  and  of  the  contact  force  along  horizontal  di¬ 
rection  with  parallel  force/position  control  scheme  for  two  environments  of  different 
compliance 


With  regard  to  the  scheme  with  inner  velocity  loop  of  Fig.  9.7,  the  proportional 
control  action  in  cfx  is 

f'Fi  =  0.0024 

so  that  the  overall  system,  for  the  more  compliant  environment,  has  two  complex 
poles  (—2.5,  ±j7.34). 

With  regard  to  the  parallel  control  scheme  of  Fig.  9.8,  the  PI  control  action  cfx 
is  chosen  with  the  same  parameters  as  for  the  first  control  scheme. 

Figures  9.9,  9.10,  9.11  report  the  time  history  of  the  tip  position  and  contact 
force  along  axis  xq  for  the  three  considered  schemes.  A  comparison  between  the 
various  cases  shows  what  follows: 

•  All  control  laws  guarantee  a  steady-state  value  of  contact  forces  equal  to  the 
desired  one  for  both  the  more  compliant  ( dashed  line )  and  the  less  compliant 
( continuous  line)  environment. 

•  For  given  motion  control  actions  (Md,  Kd,  Kp),  the  force  control  with  inner 
velocity  loop  presents  a  faster  dynamics  than  that  of  the  force  control  with  inner 
position  loop. 

•  The  dynamic  response  with  the  parallel  control  shows  how  the  addition  of  a  po¬ 
sition  reference  along  the  horizontal  direction  degrades  the  transient  behaviour, 


384  9  Force  Control 


but  it  does  not  influence  the  steady-state  contact  force.  This  effect  can  be  jus¬ 
tified  by  noting  that  a  step  position  input  is  equivalent  to  a  properly  filtered 
impulse  force  input. 

The  reference  position  along  axis  yo  is  obviously  reached  by  the  arm  tip  accord¬ 
ing  to  dynamics  of  position  control;  the  relative  time  history  is  not  reported. 


9.5  Constrained  Motion 

Force  control  schemes  can  be  employed  for  the  execution  of  a  constrained 
motion  as  long  as  they  suitably  take  into  account  the  geometric  features  of  the 
environment  and  the  force  and  position  references  are  chosen  to  be  compatible 
with  those  features. 

A  real  manipulation  task  is  characterized  by  complex  contact  situations 
where  some  directions  are  subject  to  end-effector  pose  constraints  and  others 
are  subject  to  interaction  force  constraints.  During  task  execution,  the  nature 
of  constraints  may  vary  substantially. 

The  need  to  handle  complex  contact  situations  requires  the  capacity  to 
specify  and  perform  control  of  both  end-effector  pose  and  contact  force.  How¬ 
ever,  a  fundamental  aspect  to  be  considered  is  that  it  is  not  possible  to  impose 
simultaneously  arbitrary  values  of  pose  and  force  along  each  direction.  More¬ 
over,  one  should  ensure  that  the  reference  trajectories  for  the  control  system 
be  compatible  with  the  constraints  imposed  by  the  environment  during  task 
execution. 

For  the  above  reasons,  it  is  useful  to  have  an  analytical  description  of  the 
interaction  forces,  which  is  very  demanding  from  a  modelling  point  of  view. 

A  real  contact  situation  is  a  naturally  distributed  phenomenon  in  which 
the  local  characteristics  of  the  contact  surfaces  as  well  as  the  global  dynamics 
of  the  manipulator  and  environment  are  involved.  In  detail: 

•  The  environment  imposes  kinematic  constraints  on  the  end-effector  mo¬ 
tion,  due  to  one  or  more  contacts  of  different  type;  reaction  forces  and 
moments  arise  when  the  end-effector  tends  to  violate  the  constraints  (e.g., 
the  case  of  a  robot  sliding  a  rigid  tool  on  a  frictionless  rigid  surface) . 

•  The  end-effector,  while  being  subject  to  kinematic  constraints,  may  also 
exert  dynamic  forces  and  moments  on  the  environment,  in  the  presence 
of  environment  dynamics  (e.g.,  the  case  of  a  robot  turning  a  crank,  when 
the  crank  dynamics  is  relevant,  or  a  robot  pushing  against  a  compliant 
surface). 

•  The  contact  force  and  moment  may  depend  on  the  structural  compliance 
of  the  robot,  due  to  the  finite  stiffness  of  the  joints  and  links  of  the  ma¬ 
nipulator,  as  well  as  of  the  wrist  force/torque  sensor  or  of  the  tool  (e.g.  an 
end-effector  mounted  on  an  RCC  device). 
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•  Local  deformations  of  the  contact  surfaces  may  occur  during  the  interac¬ 
tion,  producing  distributed  contact  areas;  moreover,  static  and  dynamic 
friction  may  be  present  in  the  case  of  non- ideally  smooth  contact  surfaces. 

The  design  of  the  interaction  control  is  usually  carried  out  under  simpli¬ 
fying  assumptions.  The  following  two  cases  are  considered: 

•  The  robot  and  the  environment  are  perfectly  rigid  and  purely  kinematics 
constraints  are  imposed  by  the  environment. 

•  The  robot  is  perfectly  rigid,  all  the  compliance  of  the  system  is  localized 
in  the  environment  and  the  contact  force  and  moment  is  approximated  by 
a  linear  elastic  model. 

In  both  cases,  frictionless  contact  is  assumed.  It  is  obvious  that  these  situ¬ 
ations  are  only  ideal.  However,  the  robustness  of  the  control  should  be  able  to 
cope  with  situations  where  some  of  the  ideal  assumptions  are  relaxed.  In  that 
case  the  control  laws  may  be  adapted  to  deal  with  non-ideal  characteristics. 


9.5.1  Rigid  Environment 

The  kinematic  constraints  imposed  by  the  environment  can  be  represented 
by  a  set  of  algebraic  equations  that  the  variables  describing  the  end-effector 
position  and  orientation  must  satisfy;  since  these  variables  depend  on  the  joint 
variables  through  the  direct  kinematic  equations,  the  constraint  equations  can 
also  be  expressed  in  the  joint  space  as 

<p(q)  =  0.  (9.50) 

Vector  ip  is  an  (mx  1)  function,  with  m  <  n,  where  n  is  the  number  of  joints  of 
the  manipulator,  assumed  to  be  nonredundant;  without  loss  of  generality,  the 
case  n  =  6  is  considered.  The  constraints  of  the  form  (9.50),  involving  only  the 
generalized  coordinates  of  the  system,  are  known  as  holonomic  constraints. 
Computing  the  time  derivative  of  (9.50)  yields 

Jv(q)q=0 ,  (9.51) 

where  Jv(q)  =  dp/dq  is  the  (m  x  6)  Jacobian  of  <p(q),  known  as  constraint 
Jacobian.  It  is  assumed  that  is  of  rank  m  at  least  locally  in  a  neighborhood 
of  the  operating  point;  equivalently,  the  m  constraint  equations  (9.50)  are 
assumed  to  be  locally  independent. 

In  the  absence  of  friction,  the  interaction  forces  are  reaction  forces  aris¬ 
ing  when  the  end-effector  tends  to  violate  the  constraints.  These  end-effector 
forces  produce  reaction  torques  at  the  joints  that  can  be  computed  using  the 
principle  of  virtual  work,  taking  into  account  that  the  work  of  the  reaction 
forces,  by  definition,  should  be  null  for  all  virtual  displacements  which  satisfy 
the  constraints.  Considering  the  expression  (3.108)  of  the  virtual  work  of  the 


386  9  Force  Control 


joint  torques  t  and  that,  in  view  of  (9.51),  the  virtual  displacement  Sq  satisfy 
the  equation 

Jv(o)Sq  =  0, 


yields 

r  =  Jl(q)  A, 

where  A  is  a  suitable  (m  x  1)  vector.  The  corresponding  forces  applied  to  the 
end-effector  are 

he  =  J-T(q)r  =  Sf(q)  A,  (9.52) 

assuming  a  nonsingular  J,  with 


Sf  =  J~T(q)jT(q). 


(9.53) 


Notice  that  Eq.  (9.50)  corresponds  to  a  set  of  bilateral  constraints.  This 
means  that  the  reaction  forces  (9.52)  act  so  that,  during  the  motion,  the 
end-effector  always  keeps  contact  with  the  environment,  as  for  the  case  of  a 
gripper  turning  a  crank.  However,  in  many  applications,  the  interaction  with 
the  environment  corresponds  to  unilateral  constraints.  For  example,  in  the 
case  of  a  tool  sliding  on  a  surface,  the  reaction  forces  arise  only  when  the 
tool  pushes  against  the  surface  and  not  when  it  tends  to  detach.  However, 
Eq.  (9.52)  can  be  still  applied  under  the  assumption  that  the  end-effector, 
during  the  motion,  never  loses  contact  with  the  environment. 

From  (9.52)  it  follows  that  he  belongs  to  the  m-dimensional  subspace 
IZ(Sf).  The  inverse  of  the  linear  transformation  (9.52)  can  be  computed  as 


A  =  S\(q)he, 


(9.54) 


where  Sj  denotes  a  weighted  pseudo-inverse  of  matrix  S / ,  namely 


S\  =  {S]-W  S f)-1  STfW ,  (9.55) 

where  W  is  a  symmetric  and  positive  definite  weighting  matrix. 

Notice  that,  while  subspace  7 Z(Sf)  is  uniquely  defined  by  the  geometry 
of  the  contact,  matrix  Sf  in  (9.53)  is  not  unique,  because  constraint  equa¬ 
tions  (9.50)  are  not  uniquely  defined.  Moreover,  in  general,  the  physical  di¬ 
mensions  of  the  elements  of  vector  A  are  not  homogeneous  and  the  columns  of 
matrix  Sf,  as  well  as  of  matrix  S  j,  do  not  necessarily  represent  homogeneous 
entities.  This  may  produce  invariance  problems  in  the  transformation  (9.54)  if 
he  represents  a  quantity  that  is  subject  to  disturbances  and,  as  a  result,  may 
have  components  outside  1Z(S  f).  In  particular,  if  a  physical  unit  or  a  reference 
frame  is  changed,  matrix  S /  undergoes  a  transformation;  however,  the  result 
of  (9.54)  with  the  transformed  pseudo-inverse,  in  general,  depends  on  the 
adopted  physical  units  or  reference  frame!  The  reason  is  that,  if  he  IZ(Sf), 
the  problem  of  computing  A  from  (9.52)  does  not  have  a  solution.  In  this  case, 
Eq.  (9.54)  represents  only  an  approximate  solution  which  minimizes  the  norm 
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of  vector  he  —  Sf(q)X  weighted  by  matrix  W  3  It  is  evident  that  the  invari¬ 
ance  of  the  solution  can  be  ensured  only  if,  in  the  case  that  a  physical  unit  or 
a  reference  frame  is  changed,  the  weighting  matrix  is  transformed  accordingly. 
In  the  ideal  case  he  G  Ti(Sf),  the  computation  of  the  inverse  of  (9.52)  has  a 
unique  solution,  defined  by  (9.54),  regardless  the  weighting  matrix;  hence  the 
invariance  problem  does  not  occur. 

In  order  to  guarantee  invariance,  it  is  convenient  choosing  matrix  S f  so 
that  its  columns  represent  linearly  independent  forces.  This  implies  that  (9.52) 
gives  he  as  a  linear  combination  of  forces  and  A  is  a  dimensionless  vector. 
Moreover,  a  physically  consistent  norm  in  the  space  of  forces  can  be  defined 
based  on  the  quadratic  form  h^Che,  which  has  the  meaning  of  an  elastic 
energy  if  C  is  a  positive  definite  compliance  matrix.  Hence,  the  choice  W  =  C 
can  be  made  for  the  weighting  matrix  and,  if  a  physical  unit  or  a  reference 
frame  is  changed,  the  transformations  to  be  applied  to  matrices  S /  and  W 
can  be  easily  found  on  the  basis  of  their  physical  meaning. 

Notice  that,  for  a  given  Sf,  the  constraint  Jacobian  can  be  computed 
from  (9.53)  as  Jv(q)  =  S^J^q);  moreover,  if  necessary,  the  constraint  equa¬ 
tions  can  be  derived  by  integrating  (9.51). 

By  using  (3.4),  (9.53),  equality  (9.51)  can  be  rewritten  in  the  form 

Jv{q)J-\q)J{q)q  =  STfve  =  0,  (9.56) 

which,  by  virtue  of  (9.52),  is  equivalent  to 

hTeve  =  0.  (9.57) 

Equation  (9.57)  represents  the  kinetostatic  relationship,  known  as  reciprocity , 
between  the  interaction  force  and  moment  he  belonging  to  the  so-called 

force  controlled  subspace  —  which  coincides  with  1Z(S /)  and  the  end-effector 

linear  and  angular  velocity  ve  —  belonging  to  the  so-called  velocity  controlled 
subspace.  The  concept  of  reciprocity  expresses  the  physical  fact  that,  under 
the  assumption  of  rigid  and  frictionless  contact,  the  forces  do  not  produce 
any  work  for  all  the  end-effector  displacements  which  satisfy  the  constraints. 
This  concept  is  often  confused  with  the  concept  of  orthogonality,  which  is 
meaningless  in  this  case  because  velocities  and  forces  are  non-homogeneous 
quantities  belonging  to  different  vector  spaces. 

Equations  (9.56),  (9.57)  imply  that  the  dimension  of  the  velocity  controlled 
subspace  is  6  —  m  whereas  the  dimension  of  the  force  controlled  subspace  is  in; 
moreover,  a  (6  x  (6  —  m))  matrix  Sv  can  be  defined,  which  satisfies  equation 

S^(q)Sv(q)  =  0  (9.58) 

and  such  that  TZ(SV)  represents  the  velocity  controlled  subspace.  Therefore: 

ve  =  Sv(q)u,  (9.59) 

3  See  Sect.  A. 7  for  the  computation  of  an  approximate  solution  based  on  the  left 
pseudo-inverse  and  Problem  9.5. 


388  9  Force  Control 


where  v  denotes  a  suitable  ((6  —  to)  x  1)  vector. 

The  inverse  of  the  linear  transformation  (9.59)  can  be  computed  as 

u=Sl(q)ve,  (9.60) 

where  S],  denotes  a  suitable  weighted  pseudo-inverse  of  matrix  Sv,  computed 
as  in  (9.55).  Notice  that,  as  for  the  case  of  Sf,  although  the  subspace  Ti(Sv) 
is  uniquely  defined,  the  choice  of  matrix  Sv  itself  is  not  unique.  Moreover, 
about  Eq.  (9.60),  invariance  problems  analogous  to  that  considered  for  the 
case  of  (9.54)  can  be  observed.  In  this  case,  it  is  convenient  to  select  the  matrix 
Sv  so  that  its  columns  represent  a  set  of  independent  velocities;  moreover,  for 
the  computation  of  the  pseudo-inverse,  a  norm  in  the  space  of  velocities  can 
be  defined  based  on  the  kinetic  energy  of  a  rigid  body  or  on  the  elastic  energy 
expressed  in  terms  of  the  stiffness  matrix  K  =  C-1. 

Matrix  Sv  may  also  have  an  interpretation  in  terms  of  Jacobian.  In  fact, 
due  to  the  presence  of  m  independent  holonomic  constraints  (9.50),  the  con¬ 
figuration  of  a  manipulator  in  contact  with  the  environment  can  be  locally 
described  in  terms  of  a  ((6  —  to)  x  1)  vector  r  of  independent  coordinates. 
From  the  implicit  function  theorem,  this  vector  can  be  defined  as 

r  =  ip(q),  (9.61) 

where  tp(q)  is  any  ((6  —  to)  x  1)  vector  function  such  that  the  to  components 
of  cf)(q)  and  the  6  —  to  components  of  ip{q)  are  linearly  independent  at  least 
locally  in  a  neighborhood  of  the  operating  point.  This  means  that  the  map¬ 
ping  (9.61),  together  with  the  constraint  equations  (9.50),  is  locally  invertible, 
with  inverse  defined  as 

q  =  p(r).  (9.62) 

Equation  (9.62)  explicitly  provides  all  the  joint  vectors  q  which  satisfy  the 
constraint  equations  (9.50),  for  any  r  arbitrary  selected  in  a  neighborhood 
of  the  operating  point.  Moreover,  the  vector  q  that  satisfies  (9.51)  can  be 
computed  as 

q  =  Jp(r)r, 

where  J p(r)  =  dp/dr  is  a  (6  x  (6  —  to))  full  rank  Jacobian  matrix.  Also,  the 
following  equality  holds: 

J<p(Q)Jp(r)  =  O, 

which  can  be  interpreted  as  a  reciprocity  condition  between  the  subspace 
7?.(J^)  of  the  joint  torques  r  corresponding  to  the  reaction  forces  acting  on 
the  end-effector  and  the  subspace  1Z(JP)  of  the  joint  velocities  q  which  satisfy 
the  constraints. 

The  above  equation  can  be  rewritten  as 

Jlp(q)J-1(q)J(q)Jp(r)  =  0. 
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Hence,  assuming  that  J  is  nonsingular  and  taking  into  account  (9.53),  (9.58), 
matrix  Sv  can  be  computed  as 

Sv  =  J(q)Jp(r).  (9.63) 

The  matrices  Sf,  Sv  and  the  corresponding  pseudo- inverse  matrices  Sf, 
S'v  are  known  as  selection  matrices.  These  matrices  play  a  fundamental  role 
for  task  specification,  since  they  can  be  used  to  assign  the  desired  end-effector 
motion  and  the  interaction  forces  and  moments  consistently  with  the  con¬ 
straints.  Also,  they  are  essential  for  control  synthesis. 

To  this  end,  notice  that  the  (6  x  6)  matrix  Pf  =  SfS J,  projects  a  generic 
force  vector  he  on  the  force  controlled  subspace  IZ(Sf).  Matrix  Pf  is  idem- 
potent,  namely  P2f  =  PfPf  =  Pf,  and  therefore  is  a  projection  matrix. 
Moreover,  matrix  (Jg  —  P f)  projects  force  vector  he  on  the  orthogonal  com¬ 
plement  of  the  force  controlled  subspace;  also,  this  matrix,  being  idempotent, 
it  is  a  projection  matrix. 

Similarly,  it  can  be  verified  that  the  (6  x  6)  matrices  Pv  =  SVS \  and  (Jg  — 
Pv )  are  projection  matrices,  projecting  a  generic  linear  and  angular  velocity 
vector  ve  on  the  controlled  velocity  subspace  1Z(SV)  and  on  its  orthogonal 
complement. 


9.5.2  Compliant  Environment 

In  many  applications,  the  interaction  forces  between  the  end-effector  and  a 
compliant  environment  can  be  approximated  by  the  ideal  elastic  model  of  the 
form  (9.21).  If  the  stiffness  matrix  K  is  positive  definite,  this  model  corre¬ 
sponds  to  a  fully  constrained  case  and  the  environment  deformation  coincides 
with  the  elementary  end-effector  displacement.  In  general,  however,  the  end- 
effector  motion  is  only  partially  constrained  by  the  environment  and  this  situ¬ 
ation  can  be  modelled  by  introducing  a  suitable  positive  semi-definite  stiffness 
matrix. 

This  kind  of  situation,  even  for  a  simple  case,  has  been  already  considered 
in  previous  examples  concerning  the  interaction  with  an  elastically  compliant 
plane.  In  a  general  case,  the  stiffness  matrix  describing  the  partially  con¬ 
strained  interaction  can  be  computed  by  modelling  the  environment  as  a  pair 
of  rigid  bodies,  S  and  R ,  connected  through  an  ideal  six-DOF  spring,  and 
assuming  that  the  end-effector  may  slide  on  the  external  surface  of  body  S. 

Moreover,  two  reference  frames  are  introduced,  one  attached  to  S  and  one 
attached  to  R.  At  equilibrium,  corresponding  to  the  undeformed  spring,  the 
end-effector  frame  is  assumed  to  be  coincident  with  the  frames  attached  to  S 
and  R.  The  selection  matrices  S /  and  Sv  and  the  corresponding  controlled 
force  and  velocity  subspaces  can  be  identified  on  the  basis  of  the  geometry  of 
the  contact  between  the  end-effector  and  the  environment. 
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Assumed  frictionless  contact,  the  interaction  force  applied  by  the  end- 
effector  on  body  S  belongs  to  the  force  controlled  subspace  7 Z(S f)  and  thus 

he  =  Sf\ ,  (9.64) 

where  A  is  a  (m  x  1)  vector.  Due  to  the  presence  of  the  generalized  spring,  the 
above  force  causes  a  deformation  of  the  environment  that  can  be  computed 
as 

dav.s  =  Che,  (9.65) 

where  C  is  the  compliance  matrix  of  the  spring  between  S  and  i?,  assumed 
to  be  nonsingular.  On  the  other  hand,  the  elementary  displacement  of  the 
end-effector  with  respect  to  the  equilibrium  pose  can  be  decomposed  as 

dxrte  =  dxv  +  dxf,  (9.66) 

where 

dxv  =  Pvdxrte  (9.67) 

is  the  component  belonging  to  the  velocity  controlled  subspace  1Z(SV),  where 
the  end-effector  may  slide  on  the  environment,  whereas 

dxf  =  (Iq  —  Pv)dxr^e  =  (1 6  —  Pv)dxT}S  (9.68) 

is  the  component  corresponding  to  the  deformation  of  the  environment.  Notice 
that,  in  general,  Pvdxr j6  ^  PvdxT)S. 

Premultiplying  both  sides  of  (9.66)  by  matrix  S J  and  using  (9.67),  (9.68), 
(9.65),  (9.64)  yields 

STfdxrte  =  STfdxTiS  =  STfCSf\ , 

where  the  equality  Pv  =  O  has  been  taken  into  account.  The  above  equa¬ 
tion  can  be  used  to  compute  vector  A  which,  replaced  into  (9.64),  yields 

he  =  K'dxre,  (9.69) 

where 

K'  =  SfiS^CSf)-^^  (9.70) 

is  the  positive  semi-definite  stiffness  matrix  corresponding  to  the  partially 

constrained  elastic  interaction. 

Expression  (9.70)  is  not  invertible.  However,  using  Eqs.  (9.68),  (9.65),  the 
following  equality  can  be  derived: 

dxf  =  C'he,  (9.71) 


where  the  matrix 


c  =  (l6  -  PV)C, 


(9.72) 
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of  rank  6  —  m,  has  the  meaning  of  compliance  matrix. 

Notice  that  contact  between  the  manipulator  and  the  environment  may  be 
compliant  along  some  directions  and  rigid  along  other  directions.  Therefore, 
the  force  control  subspace  can  be  decomposed  into  two  distinct  subspaces,  one 
corresponding  to  elastic  forces  and  the  other  corresponding  to  reaction  forces. 
Matrices  K'  and  C'  should  be  modified  accordingly. 


9.6  Natural  and  Artificial  Constraints 

An  interaction  task  can  be  assigned  in  terms  of  a  desired  end-effector  force  hd 
and  velocity  Vd ■  In  order  to  be  consistent  with  the  constraints,  these  vectors 
must  lie  in  the  force  and  velocity  controlled  subspaces  respectively.  This  can 
be  guaranteed  by  specifying  vectors  A^  and  i>d  and  computing  hd  and  Vd  as 


hd  —  Sf\d ,  Vd  —  SvVd, 

where  Sf  and  Sv  have  to  be  suitably  defined  on  the  basis  of  the  geometry 
of  the  task.  Therefore  vectors  A d  and  Ud  will  be  termed  ‘desired  force’  and 
‘desired  velocity’  respectively. 

For  many  robotic  tasks  it  is  possible  to  define  an  orthogonal  reference 
frame,  eventually  time- varying,  where  the  constraints  imposed  by  the  environ¬ 
ment  can  be  easily  identified,  making  task  specification  direct  and  intuitive. 
This  reference  frame  Oc-xcyczc  is  known  as  constraint  frame. 

Two  DOFs  correspond  to  each  axis  of  the  constraint  frame:  one  associ¬ 
ated  with  the  linear  velocity  or  to  the  force  along  the  axis  direction  and  the 
other  associated  with  the  angular  velocity  and  to  the  moment  along  the  axis 
direction. 

For  a  given  constraint  frame,  in  the  case  of  rigid  environment  and  absence 
of  friction,  it  can  be  observed  that: 

•  Along  each  DOF,  the  environment  imposes  to  the  manipulator’s  end- 
effector  either  a  velocity  constraint  —  in  the  sense  that  it  does  not  allow 
translation  along  a  direction  or  rotation  about  an  axis  —  or  a  force  con¬ 
straint  —  in  the  sense  that  it  does  not  allow  the  application  of  any  force 
along  a  direction  or  any  torque  about  an  axis;  such  constraints  are  termed 
natural  constraints  since  they  are  determined  directly  by  task  geometry. 

•  The  manipulator  can  control  only  the  variables  which  are  not  subject  to 
natural  constraints;  the  reference  values  for  those  variables  are  termed 
artificial  constraints  since  they  are  imposed  with  regard  to  the  strategy 
for  executing  the  given  task. 

Notice  that  the  two  sets  of  constraints  are  complementary,  in  that  they 
regard  different  variables  for  each  DOF.  Also,  they  allow  a  complete  specifi¬ 
cation  of  the  task,  since  they  involve  all  variables. 
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Fig.  9.12.  Sliding  of  a  prismatic  object  on  a  planar  surface 


In  the  case  of  compliant  environment,  for  each  DOF  where  interaction 
occurs,  one  can  choose  the  variable  to  control,  namely  force  or  velocity,  as 
long  as  the  complementarity  of  the  constraints  is  preserved.  In  case  of  high 
stiffness,  it  is  advisable  to  choose  the  force  as  an  artificial  constraint  and  the 
velocity  as  a  natural  constraint,  as  for  the  case  of  rigid  environment.  Vice 
versa,  in  the  case  of  low  stiffness,  it  is  convenient  to  make  the  opposite  choice. 
Notice  also  that,  in  the  presence  of  friction,  forces  and  moments  also  arise 
along  the  DOFs  corresponding  to  force  natural  constraints. 

9.6.1  Analysis  of  Tasks 

To  illustrate  description  of  an  interaction  task  in  terms  of  natural  and  artificial 
constraints  as  well  as  to  emphasize  the  opportunity  to  use  a  constraint  frame 
for  task  specification,  in  the  following  a  number  of  typical  case  studies  are 
analyzed. 

Sliding  on  a  planar  surface 

The  end-effector  manipulation  task  is  the  sliding  of  a  prismatic  object  on 
a  planar  surface.  Task  geometry  suggests  choosing  the  constraint  frame  as 
attached  to  the  contact  plane  with  an  axis  orthogonal  to  the  plane  (Fig.  9.12). 
Alternatively,  the  task  frame  can  be  chosen  with  the  same  orientation  but 
attached  to  the  object. 

Natural  constraints  can  be  determined  first,  assuming  rigid  and  friction¬ 
less  contact.  Velocity  constraints  describe  the  impossibility  to  generate  a  linear 
velocity  along  axis  zc  and  angular  velocities  along  axes  xc  and  yc.  Force  con¬ 
straints  describe  the  impossibility  to  exert  forces  along  axes  xc  and  yc  and  a 
moment  along  axis  zc. 

The  artificial  constraints  regard  the  variables  not  subject  to  natural  con¬ 
straints.  Hence,  with  reference  to  the  natural  constraints  of  force  along  axes 
xc,  yc  and  moment  along  zc,  it  is  possible  to  specify  artificial  constraints  for 
linear  velocity  along  xc,  yc  and  angular  velocity  along  zc.  Similarly,  with  refer¬ 
ence  to  natural  constraints  of  linear  velocity  along  axis  zc  and  angular  velocity 
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about  axes  xc  and  yc,  it  is  possible  to  specify  artificial  constraints  for  force 
along  zc  and  moments  about  xc  and  yc.  The  set  of  constraints  is  summarized 
in  Table  9.1. 


Table  9.1.  Natural  and  artificial  constraints  for  the  task  of  Fig.  9.12 


Natural 

constraints 

Artificial 

constraints 

ocz 

n 

ucx 

fl 

6% 

ft 

°y 

& 

For  this  task,  the  dimension  of  the  force  controlled  subspace  is  to  =  3, 
while  the  dimension  of  the  velocity  controlled  subspace  is  6— to  =  3.  Moreover, 
matrices  S f  and  Sv  can  be  chosen  as 

-0  0  01  rl  0  0- 

0  0  0  0  1  0 

_  1  0  0  _  0  0  0 

0  1  0  bv  ~  0  0  0  ‘ 

0  0  1  0  0  0 

.0  o  oJ  Lo  0  1. 

Notice  that,  if  the  constraint  frame  is  chosen  attached  to  the  contact  plane, 
matrices  S f  and  Sv  remain  constant  if  referred  to  the  base  frame  but  are 
time- varying  if  referred  to  the  end-effector  frame.  Vice  versa,  if  the  constraint 
frame  is  chosen  attached  to  the  object,  such  matrices  are  constant  if  referred 
to  the  end-effector  frame  and  time- varying  if  referred  to  the  base  frame. 

In  the  presence  of  friction,  non-null  force  and  moment  may  also  arise  along 
the  velocity  controlled  DOFs. 

In  the  case  of  compliant  plane,  elastic  forces  and  torques  may  be  applied 
along  the  axis  zc  and  about  the  axes  xc  and  yc  respectively,  corresponding  to 
end-effector  displacements  along  the  same  DOFs.  On  the  basis  of  the  expres¬ 
sions  derived  for  Sf  and  Sv,  the  elements  of  the  stiffness  matrix  K'  corre¬ 
sponding  to  the  partially  constrained  interaction  are  null  with  the  exception 
of  those  of  the  (3  x  3)  block  K'm  obtained  selecting  the  rows  3,  4  and  5  of  K1 . 


This  block  matrix  can  be  computed 

as 

C3,3 

C3,4 

C3,5 

-1 

K'm  =  C4i  3 

C4,4 

C4,5 

_c5,3 

c5,4 

Q>,5  _ 

where  Cij  =  Cj ^  are  the  elements  of  the  (6  x  6)  compliant  matrix  C . 
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Fig.  9.13.  Insertion  of  a  cylindrical  peg  in  a  hole 


Peg-in-hole 

The  end-effector  manipulation  task  is  the  insertion  of  a  cylindrical  object  (peg) 
in  a  hole.  Task  geometry  suggests  choosing  the  constraint  frame  with  the  origin 
in  the  centre  of  the  hole  and  an  axis  parallel  to  the  hole  axis  (Fig.  9.13).  This 
frame  can  be  chosen  attached  either  to  the  peg  or  to  the  hole. 

The  natural  constraints  are  determined  by  observing  that  it  is  not  possible 
to  generate  arbitrary  linear  and  angular  velocities  along  axes  xc,  yc,  nor  is  it 
possible  to  exert  arbitrary  force  and  moment  along  zc.  As  a  consequence,  the 
artificial  constraints  can  be  used  to  specify  forces  and  moments  along  xc  and 
yc ,  as  well  as  linear  and  angular  velocity  along  zc.  Table  9.2  summarizes  the 
constraints. 


Table  9.2.  Natural  and  artificial  constraints  for  the  task  of  Fig.  9.13 


Natural 

constraints 

Artificial 

constraints 

AC 

SI 
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fy 
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/4 

< 

n 

0Cz 

wj 

Among  the  variables  subject  to  artificial  constraints,  6^/0  describes 
insertion  while  the  others  are  typically  null  to  effectively  perform  the  task. 
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Fig.  9.14.  Turning  a  crank 


For  this  task,  the  dimension  of  the  force  controlled  subspace  is  to  =  4, 
while  the  dimension  of  the  velocity  controlled  subspace  is  6— to  =  2.  Moreover, 
matrices  S /  and  Sv  can  be  expressed  as 
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Notice  that,  if  the  constraint  frame  is  chosen  attached  to  the  hole,  matrices  S f 
and  Sv  remain  constant  if  referred  to  the  base  frame  but  are  time-varying  if 
referred  to  the  end-effector  frame.  Vice  versa,  if  the  constraint  frame  is  chosen 
attached  to  the  peg,  such  matrices  are  constant  if  referred  to  the  end-effector 
frame  and  time-varying  if  referred  to  the  base  frame. 


Turning  a  crank 

The  end-effector  manipulation  task  is  the  turning  of  a  crank.  Task  geometry 
suggests  choosing  the  constraint  frame  with  an  axis  aligned  with  the  axis  of 
the  idle  handle  and  another  axis  aligned  with  the  crank  lever  (Fig.  9.14). 
Notice  that  in  this  case  the  constraint  frame  is  time-varying. 

The  natural  constraints  do  not  allow  the  generation  of  arbitrary  linear 
velocities  along  xc,  zc  and  angular  velocities  along  xc,  yc,  nor  arbitrary  force 
along  yc  and  moment  along  zc.  As  a  consequence,  the  artificial  constraints 
allow  the  specification  of  forces  along  xc,  zc  and  moments  along  xc,  yc,  as  well 
as  a  linear  velocity  along  yc  and  an  angular  velocity  along  zc.  The  situation 
is  summarized  in  Table  9.3. 

Among  the  variables  subject  to  artificial  constraints,  forces  and  moments 
are  typically  null  for  task  execution. 
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Table  9.3.  Natural  and  artificial  constraints  for  task  in  Fig.  9.14 


Natural 

constraints 

Artificial 

constraints 

ft 

b% 

ft 

hi 

“y 

hi 

fy 

•  C 

Oy 

hz 

UCz 

For  this  task,  the  dimension  of  the  force  controlled  subspace  is  to  =  4, 
while  the  dimension  of  the  velocity  controlled  subspace  is  6— to  =  2.  Moreover, 
matrices  S /  and  Sv  can  be  expressed  as 


-1 

0 

0 

0- 

-0 

0- 

0 

0 

0 

0 

1 

0 

0 

1 

0 

0 

0 

0 

0 

0 

1 

0 

sv  = 

0 

0 

0 

0 

0 

1 

0 

0 

.0 

0 

0 

0. 

.0 

1. 

These  matrices  are  constant  in  the  constraint  frame  but  are  time-varying  if 
referred  to  the  base  frame  or  to  the  end-effector  frame,  because  the  constraint 
frame  moves  with  respect  to  both  these  frames  during  task  execution. 


9.7  Hybrid  Force/Motion  Control 

Description  of  an  interaction  task  between  manipulator  and  environment  in 
terms  of  natural  constraints  and  artificial  constraints,  expressed  with  refer¬ 
ence  to  the  constraint  frame,  suggests  a  control  structure  that  utilizes  the 
artificial  constraints  to  specify  the  objectives  of  the  control  system  so  that 
desired  values  can  be  imposed  only  onto  those  variables  not  subject  to  nat¬ 
ural  constraints.  In  fact,  the  control  action  should  not  affect  those  variables 
constrained  by  the  environment  so  as  to  avoid  conflicts  between  control  and 
interaction  with  environment  that  may  lead  to  an  improper  system  behaviour. 
Such  a  control  structure  is  termed  hybrid  force/motion  control,  since  definition 
of  artificial  constraints  involves  both  force  and  position  or  velocity  variables. 

For  the  design  of  hybrid  control,  it  is  useful  rewriting  the  dynamic  model 
of  the  manipulator  with  respect  to  the  end-effector  acceleration 

ve  =  J{q)q  +  j{q)q. 

In  particular,  replacing  (7.127)  in  the  above  expression  yields 


Be{q)ve  +  ne(q,  q)  =  7e  -  he, 


(9.73) 
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where 

Be  =  JtBJ  1 

ne  =  J  T(Cq  +  g)  -  BeJ q. 

In  the  following,  hybrid  force/motion  control  is  presented  first  for  the  case 
of  compliant  environment  and  then  for  the  case  of  rigid  environment. 

9.7.1  Compliant  Environment 

In  the  case  of  compliant  environment,  on  the  basis  of  the  decomposition  (9.66) 
and  of  Eqs.  (9.67),  (9.71),  (9.64),  the  following  expression  can  be  found 

dxrte  =  Pvdxre  +  C'  S  f\. 

Computing  the  elementary  displacements  in  terms  of  velocity,  in  view  of  (9.59) 
and  taking  into  account  that  frame  r  is  motionless,  the  end-effector  velocity 
can  be  decomposed  as 

ve  =  Svu+  C'SfX,  (9.74) 

where  the  first  term  belongs  to  the  velocity  control  subspace  and  the  second 
term  belongs  to  its  orthogonal  complement.  All  the  quantities  are  assumed 
to  be  referred  to  a  common  reference  frame  which,  for  simplicity,  was  not 
specified. 

In  the  following,  the  base  frame  is  chosen  as  the  common  reference  frame; 
moreover,  the  contact  geometry  and  the  compliance  matrix  are  assumed  to  be 
constant,  namely  Sv  =  O,  Sf  =  O  and  C  =  O.  Therefore,  computing  the 
time  derivative  of  (9.74)  yields  the  following  decomposition  for  the  end-effector 
acceleration: 

ve  =  Svu  + C'SfX.  (9.75) 

By  adopting  the  inverse  dynamics  control  law 

7e  =  Be(q)a  +  ne(q,  q)  +  he, 

where  a  is  a  new  control  input,  in  view  of  (9.73),  the  closed-loop  equation  is 

ve  =  a.  (9.76) 

On  the  basis  of  the  decomposition  (9.75),  with  the  choice 

a  =  Svav  +  C'Sffx,  (9.77) 

a  complete  decoupling  between  force  control  and  velocity  control  can  be 
achieved.  In  fact,  replacing  (9.75)  and  (9.77)  into  (9.76)  and  premultiplying 
both  sides  of  the  resulting  equation  once  by  Si  and  once  by  Sj,  the  following 
equalities  are  obtained: 

i>  =  olv  (9.78) 

\=fx.  (9.79) 
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Fig.  9.15.  Block  scheme  of  a  hybrid  force/motion  control  for  a  compliant  environ¬ 
ment 


Therefore,  the  task  can  be  assigned  specifying  a  desired  force,  in  terms 
of  vector  A d(t),  and  a  desired  velocity,  in  terms  of  vector  Ud(t).  This  control 
scheme  is  referred  to  as  hybrid  force/velocity  control. 

The  desired  velocity  vd  can  be  achieved  using  the  control  law 

OLv  =  vd  +  KPv[yd  -  v)  +  Kiv  [ (i/d(c)  -  i/(s))<fc,  (9.80) 

Jo 

where  K pv  and  K p,  are  positive  definite  matrices.  Vector  v  can  be  computed 
using  (9.60),  where  the  linear  and  angular  velocity  of  the  end-effector  ve  is 
computed  from  joint  position  and  velocity  measurements. 

The  desired  force  A^  can  be  achieved  using  the  control  law 

fx  =  ki  +  Kdx( \d  -  A)  +  KPX(\d  -  A),  (9.81) 

where  Kp> \  and  Kp\  are  positive  definite  matrices.  The  implementation  of 
the  above  control  law  requires  the  computation  of  vector  A  via  (9.54),  using 
the  measurements  of  end-effector  forces  and  moments  he.  Also,  A  can  be 
computed  as 

A  =  S\he 

in  the  ideal  case  that  he  is  available. 

The  block  scheme  of  a  hybrid  force/motion  control  law  is  shown  in 
Fig.  9.15.  The  output  variables  are  assumed  to  be  the  vector  of  end-effector 
forces  and  moments  he  and  the  vector  of  end-effector  linear  and  angular  ve¬ 
locities  ve. 
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Since  force  measurements  are  often  noisy,  the  use  of  he  is  not  feasible. 
Hence,  the  feedback  of  A  is  often  replaced  by 

A  =  S\K'J(q)q,  (9.82) 

where  K'  is  the  positive  semi-definite  stiffness  matrix  (9.70). 

If  the  contact  geometry  is  known,  but  only  an  estimate  of  the  stiff¬ 
ness/compliance  of  the  environment  is  available,  control  law  (9.77)  can  be 
rewritten  in  the  form 

OL  —  SvCX.]j  T  C  Sffx, 

where  C  =  (Iq  —  PV)C  and  C  is  an  estimate  of  C . 

In  this  case,  Eq.  (9.78)  still  holds  while,  in  place  of  (9.79),  the  following 
equality  can  be  derived: 

A  =  Lffx 

where  Lf  =  (S'J C S /)-1  C S /  is  a  nonsingular  matrix.  This  implies  that 
the  force  and  velocity  control  subspaces  remain  decoupled  and  thus  velocity 
control  law  (9.80)  does  not  need  to  be  modified. 

Since  matrix  Lf  is  unknown,  it  is  not  possible  to  achieve  the  same  perfor¬ 
mance  of  the  force  control  as  in  the  previous  case.  Also,  if  vector  A  is  computed 
starting  from  velocity  measurements  using  (9.82)  with  an  estimate  of  K' ,  only 

an  estimate  A  is  available  that,  in  view  of  (9.82),  (9.70),  can  be  expressed  in 
the  form 

A=  {STfCSf)~1STfJ{q)q. 

Replacing  (9.74)  in  the  above  equation  and  using  (9.72)  yields 

\  =  LJ1\.  (9.83) 

Considering  the  control  law 

fx  =  -kDx\  +  KPX(\d-\),  (9.84) 

with  a  constant  A^,  the  dynamics  of  the  closed-loop  system  is 
A  +  kDX\  +  LfKPX  A  =  LfKPX\d, 

where  expression  (9.83)  has  been  used.  This  equation  shows  that  the  equi¬ 
librium  solution  A  =  Ad  is  also  asymptotically  stable  in  the  presence  of  an 
uncertain  matrix  Lf,  with  a  suitable  choice  of  gain  kpX  and  of  matrix  KpX. 


Example  9.4 

Consider  a  two-link  planar  arm  in  contact  with  a  purely  frictionless  elastic  plane; 
unlike  the  above  examples,  the  plane  is  at  an  angle  of  7r/4  with  axis  xo  (Fig.  9.16). 
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Fig.  9.16.  Characterization  of  constraint  frame  for  a  two-link  planar  arm  in  contact 
with  an  elastically  compliant  plane 


The  natural  choice  of  the  constraint  frame  is  that  with  axis  xc  along  the  plane  and 
axis  yc  orthogonal  to  the  plane;  the  task  is  obviously  characterized  by  two  DOFs. 
For  the  computation  of  the  analytical  model  of  the  contact  force,  reference  frames  s 
and  r  are  chosen  so  that,  in  the  case  of  null  force,  they  coincide  with  the  constraint 
frame.  In  the  presence  of  interaction,  frame  r  remains  attached  to  the  rest  position 
of  the  plane  while  frame  s  remains  attached  to  the  contact  plane  in  the  deformed 
position;  the  constraint  frame  is  assumed  to  be  attached  to  frame  s.  Matrices  Sf 
and  S%,  referred  to  the  constraint  frame,  have  the  form 


Scf  = 


scv  = 


and  the  corresponding  projection  matrices  are 


P°f  = 


o 

o 

P%  = 

1 

o' 

L° 

-*•  V 

0 

0 

In  view  of  (9.70),  (9.72),  the  stiffness  and  the  compliance  matrices,  referred  to 
the  constraint  frame,  have  the  expression 


0  0 

0  C2,2  ’ 

where  C2,2  characterizes  the  compliance  of  frame  s  with  respect  to  frame  r  along  the 
direction  orthogonal  to  the  plane,  aligned  to  axis  yc  of  the  constraint  frame. 

It  is  evident  that,  under  the  assumption  that  the  plane  is  compliant  only  along 
the  orthogonal  direction  and  that  this  direction  remains  fixed,  then  the  constraint 
frame  orientation  remains  constant  with  respect  to  the  base  frame.  The  correspond¬ 
ing  rotation  matrix  is  given  by 


K'c  = 


0 

C2,2 


cc  = 


l/s/2  — 1/\/2 
l/y/2  l/y/2 


(9.85) 


Moreover,  if  the  task  is  to  slide  the  manipulator  tip  along  the  plane,  the  end-effector 
velocity  can  be  decomposed  according  to  (9.74)  in  the  form 

vce  =  Scvv  +  ClcScf\, 


(9.86) 
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where  all  the  quantities  are  referred  to  the  constraint  frame.  It  can  be  easily  shown 
that,  ii  f  l  =  [fx  fy]T  and  v%  =  [  6%  dy  ]T,  it  is  v  =  ocx  and  A  =  fy-  This  equation 
can  also  be  referred  to  the  base  frame,  where  matrices 


Sf 


RcS cf  = 


— 1/ a/2 

l/y/2 


Sv  =  RCSCV 


1/V2' 

1/V2  ’ 


are  constant  and  the  compliance  matrix 


C'  =  RcC'CRl  =  C2.2 


1/2 

-1/2 


-1/2 

1/2 


is  constant  during  the  end-effector  motion  on  the  plane,  for  constant  02,2-  The  adop¬ 
tion  of  an  inverse  dynamics  control  law,  with  the  choice  (9.77),  leads  to 


Z>  —  Ox  —  OLi/ 

A  =  fy  =  /A, 

showing  that  hybrid  control  achieves  motion  control  along  axis  xc  and  force  con¬ 
trol  along  axis  yc,  provided  that  a„  and  f\  are  set  according  to  (9.80)  and  (9.81) 
respectively. 

Finally,  notice  that  the  formulation  of  the  control  law  can  be  further  simplified 
if  the  base  frame  is  chosen  parallel  to  the  constraint  frame. 


9.7.2  Rigid  Environment 

In  the  case  of  rigid  environment,  the  interaction  force  and  moment  can  be 
written  in  the  form  he  =  SfX.  Vector  A  can  be  eliminated  from  (9.73)  by 
solving  (9.73)  for  ve  and  substituting  it  into  the  time  derivative  of  the  equal¬ 
ity  (9.56).  This  yields 

A  =  Bf(q)  (sTfB-\q){le  -  ne(q,  q))  +  sjve ) ,  (9.87) 

where  Bf  =  (S'}’R"1S,/)'1. 

Hence,  the  dynamic  model  (9.73)  for  the  manipulator  constrained  by  the 
rigid  environment  can  be  rewritten  in  the  form 

Be(q)ve  +  SfBf{q)STfve  =  P(q)('Ye  -  we(q,  <?)),  (9-88) 

with  P  =  1 6  —  S fB  fSTf  B~x .  Notice  that  PSf  =  O ;  moreover,  this  matrix 
is  idempotent.  Therefore,  matrix  P  is  a  (6  x  6)  projection  matrix  that  filters 
out  all  the  components  of  the  end-effector  forces  lying  in  the  subspace  1Z(S /). 

Equation  (9.87)  reveals  that  vector  A  instantaneously  depends  on  the  con¬ 
trol  force  7e.  Hence,  by  suitably  choosing  7e,  it  is  possible  to  control  directly 
the  in  independent  components  of  the  end-effector  forces  that  tend  to  violate 
the  constraints;  these  components  can  be  computed  from  A,  using  (9.52). 
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On  the  other  hand,  (9.88)  represents  a  set  of  six  second  order  differen¬ 
tial  equations  whose  solution,  if  initialized  on  the  constraints,  automatically 
satisfies  Eq.  (9.50)  at  all  times. 

The  reduced-order  dynamic  model  of  the  constrained  system  is  described 
by  6  —  m  independent  equations  that  are  obtained  premultiplying  both  sides 
of  (9.88)  by  matrix  S l  and  substituting  the  acceleration  ve  with 

ve  =  Svv  +  Svv. 

Using  the  identities  (9.58)  and  S^P  =  S l  yields 

Bv(q)i>  =  Si  (7e  -  ne(q,  q)  -  Be(q)Svuj  ,  (9.89) 

where  Bv  =  Si BeSv.  Moreover,  expression  (9.87)  can  be  rewritten  as 

A  =  Bf{q)STfB-\q)  (7e  -  ne(q.  q)  -  Be(q)Svu )  ,  (9.90) 

where  the  identity  S f  Sv  =  —S  f  Sv  has  been  exploited. 

With  reference  to  (9.89),  consider  the  choice 

7e  =  Be(q)Svav  +  Sffx  +  ne{q ,  q)  +  Be(q)Sv v,  (9.91) 

where  av  and  f  x  are  new  control  inputs.  By  replacing  (9.91)  in  (9.89),  (9.90), 
the  following  two  equations  can  be  found: 


i>  =  OLv 

A  =  fx, 

showing  that  the  inverse  dynamics  control  law  (9.91)  allows  a  complete  de¬ 
coupling  between  force  and  velocity  controlled  subspaces. 

It  is  worth  noticing  that,  for  the  implementation  of  control  law  (9.91), 
constraint  equations  (9.50)  as  well  as  Eq.  (9.61)  defining  the  vector  of  the 
configuration  variables  for  the  constrained  system  are  not  required,  provided 
that  matrices  S f  and  Sv  are  known.  These  matrices  can  be  computed  on  the 
basis  of  the  geometry  of  the  environment  or  estimated  on-line,  using  force  and 
velocity  measurements. 

The  task  can  easily  be  assigned  by  specifying  a  desired  force,  in  terms 
of  vector  A,i(t),  and  a  desired  velocity,  in  terms  of  vector  Ud{t)\  the  resulting 
scheme  of  hybrid  force/velocity  control  is  conceptually  analogous  to  that  shown 
in  Fig.  9.15. 

The  desired  velocity  i 'd  can  be  achieved  by  setting  olv  according  to  (9.80), 
as  for  the  case  of  compliant  environment. 

The  desired  force  A d  can  be  achieved  by  setting 


/a  —  Ad 


(9.92) 
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but  this  choice  is  very  sensitive  to  disturbance  forces,  since  it  contains  no  force 
feedback.  Alternative  choices  are 

fx  =  \d  +  KPX{\d~\),  (9.93) 

0r  t 

fx  =  \d  +  KIX[(\d(<;)-\(<;))d<;,  (9.94) 

Jo 

where  K pX  and  K jX  are  suitable  positive  definite  matrices.  The  proportional 
feedback  is  able  to  reduce  the  force  error  due  to  disturbance  forces,  while  the 
integral  action  is  able  to  compensate  for  constant  bias  disturbances. 

The  implementation  of  force  feedback  requires  the  computation  of  vector 
A  from  the  measurement  of  the  end-effector  force  and  moment  he,  that  can 
be  achieved  using  (9.54). 

When  Eqs.  (9.50)  and  (9.61)  are  available,  matrices  Sf  and  Sv  can  be 
computed  according  to  (9.53)  and  (9.63),  respectively.  Moreover,  a  hybrid 
force/position  control  can  be  designed  specifying  a  desired  force  A d(t),  and  a 
desired  position  rd(t). 

The  force  control  law  can  be  designed  as  above,  while  the  desired  position 
rd  can  be  achieved  with  the  choice  (see  Problem  9.11) 

olv  =  rd  +  KDr(rd  -  u)  +  KPr(rd  -  r),  (9.95) 

where  K p>r  and  K Pr  are  suitable  positive  definite  matrices.  Vector  r  can  be 
computed  from  the  joint  position  measurements  using  (9.61). 
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Problems 

9.1.  Derive  expressions  (9.10),  (9.11). 

9.2.  Show  that  the  equilibrium  equations  for  the  compliance  control  scheme 
are  expressed  by  (9.22),  (9.23). 

9.3.  Consider  the  planar  arm  in  contact  with  the  elastically  compliant  plane 
in  Fig.  9.16.  The  plane  forms  an  angle  of  7t/4  with  axis  Xq  and  its  undeformed 
position  intersects  axis  xq  in  the  point  of  coordinates  (1,0);  the  environment 
stiffness  along  axis  yc  is  5  •  103N/m.  With  the  data  of  the  arm  in  Sect.  8.7, 
design  an  impedance  control.  Perform  a  computer  simulation  of  the  interaction 
of  the  controlled  manipulator  along  the  rectilinear  path  from  position  Pj  = 
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[1  0]T  to  Pf  =  [  1.2  +  O.ly/2  0.2 ]T  with  a  trapezoidal  velocity  profile  and 

a  trajectory  duration  tf  =  Is.  Implement  the  control  in  discrete-time  with  a 
sampling  time  of  1ms. 

9.4.  Show  that  the  equilibrium  position  for  the  parallel  force/position  control 
scheme  satisfies  (9.49). 

9.5.  Show  that  expression  (9.54)  with  (9.55)  is  the  solution  which  minimizes 
the  norm  \\he  —  S'/(q)A||  with  weighting  matrix  W. 

9.6.  Show  that  stiffness  matrix  (9.70)  can  be  expressed  in  the  form  K'  = 

PfK. 

9.7.  For  the  manipulation  task  of  driving  a  screw  in  a  hole  illustrated  in 
Fig.  9.17,  find  the  natural  constraints  and  artificial  constraints  with  respect 
to  a  suitably  chosen  constraint  frame. 

9.8.  Show  that  the  hybrid  control  scheme  of  Example  9.4,  in  the  force  con¬ 
trolled  subspace,  is  equivalent  to  a  force  control  scheme  with  inner  velocity 
loop. 

9.9.  For  the  arm  and  environment  of  Example  9.4  compute  the  expressions 
of  Sj  and  S\,  in  the  constraint  frame  and  in  the  base  frame. 

9.10.  For  the  arm  and  environment  of  Problem  9.3,  design  a  hybrid  control 
in  which  a  motion  control  law  operates  along  axis  xc  while  a  force  control  law 
operates  along  axis  yc;  let  the  desired  contact  force  along  axis  yc  be  50  N.  Per¬ 
form  a  computer  simulation  of  the  interaction  of  the  controlled  manipulator 
along  a  trajectory  on  the  plane  equivalent  to  that  of  Problem  9.3.  Implement 
the  control  in  discrete-time  with  a  sampling  time  of  1  ms. 

9.11.  Show  that  control  law  (9.95)  ensures  tracking  of  a  desired  position  r<j(t). 
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Visual  Servoing 


Vision  allows  a  robotic  system  to  obtain  geometrical  and  qualitative  infor¬ 
mation  on  the  surrounding  environment  to  be  used  both  for  motion  planning 
and  control.  In  particular,  control  based  on  feedback  of  visual  measurements  is 
termed  visual  servoing.  In  the  first  part  of  this  chapter,  some  basic  algorithms 
for  image  processing,  aimed  at  extracting  numerical  information  referred  to  as 
image  feature  parameters ,  are  presented.  These  parameters,  relative  to  images 
of  objects  present  in  the  scene  observed  by  a  camera,  can  be  used  to  estimate 
the  pose  of  the  camera  with  respect  to  the  objects  and  vice  versa.  To  this  end, 
analytical  pose  estimation  methods,  based  on  the  measurement  of  a  certain 
number  of  points  or  correspondences  are  presented.  Also,  numerical  pose  esti¬ 
mation  methods,  based  on  the  integration  of  the  linear  mapping  between  the 
camera  velocity  in  the  operational  space  and  the  time  derivative  of  the  fea¬ 
ture  parameters  in  the  image  plane,  are  introduced.  In  cases  in  which  multiple 
images  of  the  same  scene,  taken  from  different  viewpoints,  are  available,  addi¬ 
tional  information  can  be  obtained  using  stereo  vision  techniques  and  epipolar 
geometry.  A  fundamental  operation  is  also  camera  calibration',  to  this  end,  a 
calibration  method  based  on  the  measurement  of  a  certain  number  of  corre¬ 
spondences  is  presented.  Then,  the  two  main  approaches  to  visual  servoing 
are  introduced,  namely  position-based  visual  servoing  and  image-based  visual 
servoing,  as  well  as  a  scheme,  termed  hybrid  visual  servoing ,  which  combines 
the  benefits  of  both  approaches. 


10.1  Vision  for  Control 

Vision  plays  a  key  role  in  a  robotic  system,  as  it  can  be  used  to  obtain  ge¬ 
ometrical  and  qualitative  information  on  the  environment  where  the  robot 
operates,  without  physical  interaction.  Such  information  may  be  employed  by 
the  control  system  at  different  levels,  for  the  sole  task  planning  and  also  for 
feedback  control. 


408  10  Visual  Servoing 


As  an  example,  consider  the  case  of  a  robot  manipulator,  equipped  with  a 
camera,  which  has  to  grasp  an  object  using  a  gripper.  Through  vision  the  robot 
may  acquire  information  capable  of  identifying  the  relative  pose  of  the  object 
with  respect  to  the  gripper.  This  information  allows  the  control  system  to  plan 
a  trajectory  leading  the  manipulator  in  an  appropriate  grasping  configuration, 
computed  on  the  basis  of  the  pose  and  of  the  shape  of  the  object,  from  which 
the  closure  of  the  gripper  can  be  commanded. 

The  planned  trajectory  can  be  executed  using  a  simple  motion  controller. 
In  this  approach,  termed  look-and-move ,  visual  measurements  are  used  in  open 
loop,  making  the  system  very  sensitive  to  uncertainties  due,  for  instance,  to 
poor  positioning  accuracy  of  the  manipulator  or  to  the  fact  that  the  object 
may  have  moved  while  the  gripper  reaches  the  grasp  position. 

On  the  other  hand,  in  vision-based  control  or  visual  servoing ,  the  visual 
measurements  are  fed  back  to  the  control  to  compute  an  appropriate  error 
vector  defined  between  the  current  pose  of  the  object  and  the  pose  of  the 
manipulator’s  end-effector. 

A  key  characteristic  of  visual  servoing,  compared  to  motion  and  force  con¬ 
trol,  is  the  fact  that  the  controlled  variables  are  not  directly  measured  by  the 
sensor,  but  are  obtained  from  the  measured  quantities  through  complex  elab¬ 
orations,  based  on  algorithms  of  image  processing  and  computational  vision. 

In  Sect.  5.4.3  it  was  shown  that  a  monochrome  camera  simply  provides 
a  two-dimensional  matrix  of  values  of  light  intensity.  From  this  matrix,  the 
so-called  image  feature  parameters  are  to  be  extracted  in  real  time.  The  ge¬ 
ometric  relationships  between  one  or  more  two-dimensional  views  of  a  scene 
and  the  corresponding  3D  space  are  the  basis  of  techniques  of  pose  estimation 
of  objects  in  the  manipulator  workspace  or  of  the  end-effector  with  respect 
to  the  surrounding  objects.  In  this  regard,  of  fundamental  importance  is  the 
operation  of  camera  calibration,  which  is  necessary  for  calculating  the  intrin¬ 
sic  parameters,  relating  the  quantities  measured  in  the  image  plane  to  those 
referred  to  the  camera  frame,  and  the  extrinsic  parameters ,  relating  the  latter 
to  quantities  defined  in  a  frame  attached  to  the  manipulator. 

The  vision-based  control  schemes  can  be  divided  into  two  categories, 
namely,  those  that  realize  visual  servoing  in  operational  space,  also  termed 
position-based  visual  servoing,  and  those  that  realize  visual  servoing  in  the 
image  space,  also  known  as  image-based  visual  servoing.  The  main  difference 
lies  in  the  fact  that  the  schemes  of  the  first  category  use  visual  measurements 
to  reconstruct  the  relative  pose  of  the  object  with  respect  to  the  robot,  or  vice 
versa,  while  the  schemes  of  the  second  category  are  based  on  the  comparison 
of  the  feature  parameters  of  the  image  of  the  object  between  the  current  and 
the  desired  pose.  There  are  also  schemes  combining  characteristics  common 
to  both  categories,  that  can  be  classified  as  hybrid  visual  servoing. 

Another  aspect  to  be  considered  for  vision-based  control  is  the  type  of 
camera  (colour  or  monochrome,  resolution,  fixed  or  variable  focal  length,  CCD 
or  CMOS  technology).  In  this  chapter,  only  the  case  of  monochrome  cameras 
with  fixed  focal  length  will  be  considered. 
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Equally  important  is  the  choice  of  the  number  of  cameras  composing  the 
visual  system  and  their  location;  this  issue  is  briefly  discussed  in  the  following. 


10.1.1  Configuration  of  the  Visual  System 

A  visual  system  may  consist  of  only  one  camera,  or  two  or  more  cameras.  If 
more  cameras  are  used  to  observe  the  same  object  of  a  scene,  it  is  possible  to 
retrieve  information  about  its  depth  by  evaluating  its  distance  with  respect  to 
the  visual  system.  This  situation  is  referred  to  as  3D  vision  or  stereo  vision, 
where  the  term  stereo  derives  from  the  Greek  and  means  solid.  The  human 
capability  of  perceiving  objects  in  three  dimensions  relies  on  the  fact  that  the 
brain  receives  the  same  images  from  two  eyes,  observing  the  same  scene  from 
slightly  different  angles. 

It  is  clear  that  3D  vision  can  be  achieved  even  with  one  camera,  provided 
that  two  images  of  the  same  object,  taken  from  two  different  poses,  are  avail¬ 
able.  If  only  a  single  image  is  available,  the  depth  can  be  estimated  on  the  ba¬ 
sis  of  certain  geometrical  characteristics  of  the  object  known  in  advance.  This 
means  that,  in  many  applications,  mono-camera  systems  are  often  preferred 
to  multi-camera  systems,  because  they  are  cheaper  and  easier  to  calibrate, 
although  characterized  by  lower  accuracy. 

Another  feature  that  distinguishes  visual  systems  for  robot  manipulators 
is  the  placement  of  cameras.  For  mono-camera  systems  there  are  two  options: 
the  fixed  configuration,  often  referred  to  as  eye-to-hand,  where  the  camera  is 
mounted  in  a  fixed  location,  and  the  mobile  configuration,  or  eye-in-liand ,  with 
the  camera  attached  to  the  robot.  For  multi-camera  systems,  in  addition  to  the 
mentioned  solutions,  it  is  also  possible  to  consider  the  hybrid  configuration, 
consisting  of  one  or  more  cameras  in  eye-to-hand  configuration,  and  one  or 
more  cameras  in  eye-in-hand  configuration. 

In  the  eye-to-hand  configuration,  the  visual  system  observes  the  objects  to 
be  manipulated  by  a  fixed  pose  with  respect  to  the  base  frame  of  the  manipu¬ 
lator.  The  advantage  is  that  the  camera  field  of  view  does  not  change  during 
the  execution  of  the  task,  implying  that  the  accuracy  of  such  measurements 
is,  in  principle,  constant.  However,  in  certain  applications,  such  as  assembly, 
it  is  difficult  to  prevent  that  the  manipulator,  moving  in  the  camera  field  of 
view,  occludes,  in  part  or  in  whole,  the  view  of  the  objects. 

In  the  eye-in-hand  configuration,  the  camera  is  placed  on  the  manipula¬ 
tor  and  can  be  mounted  both  before  and  after  the  wrist.  In  the  first  case, 
the  camera  can  observe  the  end-effector  by  a  favourable  pose  and  without 
occlusions  caused  by  the  manipulator  arm;  in  the  latter  case,  the  camera  is 
attached  to  the  end-effector  and  typically  observes  only  the  object.  In  both 
situations,  the  camera  held  of  view  changes  significantly  during  the  motion 
and  this  produces  a  high  variability  in  the  accuracy  of  measurements.  How¬ 
ever,  when  the  end-effector  is  close  to  the  object,  the  accuracy  becomes  almost 
constant  and  is  usually  higher  than  that  achievable  with  eye-to-hand  cameras, 
with  the  advantage  that  occlusions  are  virtually  absent. 
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Finally,  hybrid  configuration  combines  the  benefits  of  the  other  two  con¬ 
figurations,  namely,  ensures  a  good  accuracy  throughout  the  workspace,  while 
avoiding  the  problems  of  occlusions. 

A  separate  category  is  represented  by  robotic  heads,  which  are  typically 
equipped  with  a  stereo  vision  system  consisting  of  two  cameras  mounted  on 
motorized  mechanisms  that  allow  for  yaw  motion,  or  pan,  and  pitch  motion, 
or  tilt ,  hence  the  name  of  pan-tilt  cameras. 

In  this  chapter,  only  schemes  based  on  a  single  eye-in-hand  camera  will 
be  considered.  The  extension  of  the  algorithms  to  the  case  of  a  eye-to-hand 
camera,  or  to  the  case  of  multiple  cameras,  requires  only  minor  modifications. 


10.2  Image  Processing 

Visual  information,  unlike  the  information  provided  by  other  types  of  sensors, 
is  very  rich  and  varied  and  thus  requires  complex  and  computational  expen¬ 
sive  transformations  before  it  can  be  used  for  controlling  a  robotic  system. 
The  objective  of  these  transformations  is  the  extraction  of  numerical  infor¬ 
mation  from  the  image,  which  provides  a  synthetic  and  robust  description 
of  the  objects  of  interest  in  the  scene,  through  the  so-called  image  feature 
parameters. 

To  this  end,  two  basic  operations  are  required.  The  first  is  so-called  segmen¬ 
tation,  which  aims  at  obtaining  a  representation  suitable  for  the  identification 
of  measurable  features  of  the  image.  The  subsequent  operation,  termed  inter¬ 
pretation  is  concerned  with  the  measurement  of  the  feature  parameters  of  the 
image. 

The  source  information  is  contained  in  a  framestore,  namely  the  two- 
dimensional  memory  array  representing  the  spatial  sample  of  the  image.  On 
the  set  of  pixels  the  so-called  image  function  is  defined  which,  in  general, 
is  a  vector  function  whose  components  represent  the  values  of  one  or  more 
physical  quantities  related  to  the  pixel,  in  a  sampled  and  quantized  form. 

For  example,  in  the  case  of  color  images,  the  image  function  defined  on 
a  pixel  of  coordinates  (. Xj,Yi )  has  three  components  Ir(Xi,Yi),  Ig(Xi,Yi) 
and  Ib(Xj,  Yi),  corresponding  to  the  light  intensity  in  the  wavelengths  of  red, 
green  and  blue.  For  a  monocrome  black-and-white  image,  the  image  function  is 
scalar  and  coincides  with  the  light  intensity  in  shades  of  gray  I(Xj,Yj),  also 
referred  to  as  gray  level.  In  the  following,  for  simplicity,  only  monochrome 
images  will  be  considered. 

The  number  of  gray  levels  depends  on  the  adopted  grey-scale  resolution. 
In  all  cases,  the  gray  scale  is  bounded  by  two  gray  levels,  black  and  white, 
corresponding  to  the  minimum  and  maximum  measurable  light  intensity  re¬ 
spectively.  Most  current  acquisition  equipments  adopt  a  scale  consisting  of 
256  gray  levels,  that  can  be  represented  by  a  single  byte  of  memory. 
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Fig.  10.1.  Black-and-white  image  and 
right 


x  104 


A  representation  of  the  framestore  which  is  particularly  useful  for  subse¬ 
quent  processing  is  the  gray-level  histogram,  which  provides  the  frequency  of 
occurrence  of  each  gray  level  in  the  image. 

Where  the  gray  levels  are  quantized  from  0  to  255,  the  value  h(p )  of  the 
histogram  at  a  particular  gray  level  p  £  [0, 255]  is  the  number  of  image  pixels 
with  gray  level  p.  If  this  value  is  divided  by  the  total  number  of  pixels,  the 
histogram  is  termed  normalized  histogram. 

Figure  10.1  shows  a  black-and-white  image  and  the  corresponding  gray- 
level  histogram.  Proceeding  from  left  to  right,  three  main  peaks  can  be  ob¬ 
served  —  from  left  to  right  —  corresponding  to  the  darkest  object,  the  lightest 
object,  and  the  background. 

10.2.1  Image  Segmentation 

Segmentation  consists  of  a  grouping  process,  by  which  the  image  is  divided 
into  a  certain  number  of  groups,  referred  to  as  segments,  so  that  the  com¬ 
ponent  of  each  group  are  similar  with  respect  to  one  or  more  characteristics. 
Typically,  distinct  segments  of  the  image  correspond  to  distinct  objects  of  the 
environment,  or  homogeneous  object  parts. 

There  are  two  complementary  approaches  to  the  problem  of  image  seg¬ 
mentation:  one  is  based  on  finding  connected  regions  of  the  image,  the  other 
is  concerned  with  detection  of  boundaries .  The  objective  of  region-based  seg¬ 
mentation  is  that  of  grouping  sets  of  pixels  sharing  common  features  into 
two-dimensional  connected  areas,  with  the  implicit  assumption  that  the  re¬ 
sulting  regions  correspond  to  real-world  surfaces  or  objects.  On  the  other 
hand,  boundary-based  segmentation  is  aimed  at  identifying  the  pixels  corre¬ 
sponding  to  object  contours  and  isolating  them  from  the  rest  of  the  image. 
The  boundary  of  an  object,  once  extracted,  can  be  used  to  define  the  position 
and  shape  of  the  object  itself. 
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The  complementarity  of  the  two  approaches  relies  on  the  fact  that  a  bound¬ 
ary  can  be  achieved  by  isolating  the  contours  of  a  region  and,  conversely,  a 
region  can  be  achieved  simply  by  considering  the  set  of  pixels  contained  within 
a  closed  boundary. 

The  problem  of  segmentation  is  not  trivial  and  there  exist  many  solutions, 
some  of  which  are  sketched  below.  From  the  point  of  view  of  memory  usage, 
boundary-based  segmentation  is  more  convenient,  since  boundaries  contain 
a  reduced  number  of  pixels.  However,  from  the  computational  load  point  of 
view,  region-based  segmentation  is  faster  because  it  requires  a  reduced  number 
of  memory  accesses. 

Region-based  segmentation 

The  central  idea  underlying  region-based  segmentation  techniques  is  that  of 
obtaining  connected  regions  by  continuous  merging  of  initially  small  groups 
of  adjacent  pixels  into  larger  ones. 

Merging  of  two  adjacent  regions  may  happen  only  if  the  pixels  belonging  to 
these  regions  satisfy  a  common  property,  termed  uniformity  predicate.  Often 
the  uniformity  predicate  requires  that  the  gray  level  of  the  pixels  of  the  region 
belongs  to  a  given  interval. 

In  many  applications  of  practical  interest  a  thresholding  approach  is 
adopted  and  a  light  intensity  scale  composed  of  only  two  values  (0  and  1) 
is  considered.  This  operation  is  referred  to  as  binary  segmentation  or  image 
binarization ,  and  corresponds  to  separating  one  or  more  objects  present  in 
the  image  from  the  background  by  comparing  the  gray  level  of  each  pixel 
with  a  threshold  l.  For  light  objects  against  a  dark  background,  all  the  pixels 
whose  gray  level  is  greater  than  the  threshold  are  considered  to  belong  to  a 
set  S0,  corresponding  to  the  objects,  while  all  the  other  pixels  are  considered 
to  belong  to  a  set  Sb  corresponding  to  the  background.  It  is  obvious  that  this 
operation  can  be  reversed  for  dark  objects  against  a  light  background.  When 
only  an  object  is  present  in  the  image,  the  segmentation  ends  with  the  detec¬ 
tion  of  sets  S0  and  Sb,  representing  two  regions;  in  the  presence  of  multiple 
objects,  a  further  elaboration  is  required  to  separate  the  connected  regions 
corresponding  to  the  single  objects.  The  image  obtained  assigning  a  light  in¬ 
tensity  equal  to  0  to  all  the  pixels  of  set  S0,  and  a  light  intensity  equal  to  1 
to  all  the  pixels  of  set  Sb,  or  vice  versa,  is  termed  binary  image. 

A  crucial  factor  for  the  success  of  binary  segmentation  is  the  choice  of  the 
threshold.  A  widely  adopted  method  for  selecting  the  threshold  is  based  on 
the  gray-level  histogram,  under  the  assumption  that  it  contains  clearly  dis¬ 
tinguishable  minimum  and  maximum  values,  corresponding  to  the  gray  levels 
of  the  objects  and  of  the  background;  the  peaks  of  the  histogram  are  also 
termed  modes.  For  dark  objects  against  a  light  background,  the  background 
corresponds  to  the  mode  which  is  located  further  to  the  right  —  as,  for  ex¬ 
ample,  in  the  case  of  Fig.  10.1  —  and  the  threshold  can  be  chosen  at  the 
closest  minimum  to  the  left.  For  light  objets  against  a  dark  background,  the 
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Fig.  10.2.  Binary  image  corresponding  to  image  of  Fig.  10.1 


background  corresponds  to  the  mode  which  is  located  further  to  the  left  and 
the  threshold  should  be  selected  accordingly.  With  reference  to  Fig.  10.1,  the 
threshold  can  be  set  to  l  =  152.  The  corresponding  binary  image  is  reported 
in  Fig.  10.2. 

In  practice,  the  gray-scale  histogram  is  noisy  and  the  modes  are  difficult 
to  identify.  Often,  there  is  no  clear  separation  between  the  gray  levels  of 
the  objects  and  those  of  the  background.  To  this  end,  various  techniques 
have  been  developed  to  increase  the  robustness  of  binary  segmentation,  which 
require  appropriate  filtering  of  the  image  before  binarization  and  the  adoption 
of  algorithms  for  automatic  selection  of  the  threshold. 


Boundary-based  segmentation 

Boundary-based  segmentation  techniques  usually  obtain  a  boundary  by  group¬ 
ing  many  single  local  edges,  corresponding  to  local  discontinuities  of  image 
gray  level.  In  other  words,  local  edges  are  sets  of  pixels  where  the  light  inten¬ 
sity  changes  abruptly. 

The  algorithms  for  boundary  detection  first  derive  an  intermediate  image 
based  on  local  edges  from  the  original  gray-scale  image,  then  they  construct 
short-curve  segments  by  edge  linking,  and  finally  obtain  the  boundaries  by 
joining  these  curve  segments  through  geometric  primitives  often  known  in 
advance. 

Boundary-based  segmentation  algorithms  vary  in  the  amount  of  a  priori 
knowledge  they  incorporate  in  associating  or  linking  the  edges  and  their  ef¬ 
fectiveness  clearly  depends  on  the  quality  of  the  intermediate  image  based 
on  local  edges.  The  more  reliable  the  local  edges  in  terms  of  their  position, 
orientation  and  ‘authenticity’,  the  easier  the  task  of  the  boundary  detection 
algorithm. 

Notice  that  edge  detection  is  essentially  a  filtering  process  and  can  often  be 
implemented  via  hardware,  whereas  boundary  detection  is  a  higher  level  task 
usually  requiring  more  sophisticated  software.  Therefore,  the  current  trend 
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is  that  of  using  the  most  effective  edge  detector  to  simplify  the  boundary 
detection  process.  In  the  case  of  simple  and  well-defined  shapes,  boundary 
detection  becomes  straightforward  and  segmentation  reduces  to  the  sole  edge 
detection. 

Several  edge  detection  techniques  exist.  Most  of  them  require  the  calcula¬ 
tion  of  the  gradient  or  of  the  Laplacian  of  function  I(Xj,Yj). 

Since  a  local  edge  is  defined  as  a  transition  between  two  regions  of  signifi¬ 
cantly  different  gray  levels,  it  is  obvious  that  the  spatial  gradient  of  function 
I(Xj,  Yi),  which  measures  the  rate  of  change  of  the  gray  level,  will  have  large 
magnitude  close  to  these  transitional  boundary  areas.  Therefore,  edge  detec¬ 
tion  can  be  performed  by  grouping  the  pixels  where  the  magnitude  of  the 
gradient  is  greater  than  a  threshold.  Moreover,  the  direction  of  the  gradient 
vector  will  be  the  direction  of  maximum  variation  of  the  gray  level. 

Again,  the  choice  of  the  value  of  the  threshold  is  extremely  important;  in 
the  presence  of  noise,  the  threshold  is  the  result  of  a  trade-off  between  the 
possibility  of  losing  valid  edges  and  that  of  detecting  false  edges. 

For  gradient  computation,  it  suffices  to  evaluate  the  directional  derivatives 
of  function  I(Xj,Yj)  along  two  orthogonal  directions.  Since  this  function  is 
defined  on  a  discrete  set  of  pixels,  the  derivatives  are  computed  in  an  approx¬ 
imate  way.  The  essential  differences  between  gradient-based  edge  detection 
techniques  are  the  directions  used  for  the  computation  of  the  derivatives  and 
the  manner  in  which  they  approximate  these  derivatives  and  compute  the 
gradient  magnitude. 

The  most  common  operator  for  the  computation  of  the  gradient  is  that 
approximating  the  derivative  along  directions  Xj  and  Yj  with  the  first  differ¬ 
ences: 


Ai  =  /(A7  +  1,17)  -  I{XI,Yi) 

A2  =  I(Xi,Yi  +  1)-I(Xi,Yi). 

Other  operators,  less  sensitive  to  noise  effects  are,  for  example,  the  Roberts 
operator ,  based  on  the  first  differences  computed  along  the  diagonals  of  a 
(2  x  2)  square  of  pixels: 

a1  =  I(XT  + 1,  17  + 1)  -  I(XT,  17) 

A2  =  I(Xj,  Yr  +  1)  -  I{Xi  +  1, 17), 

and  the  Sobel  operator ,  defined  on  a  (3  x  3)  square  of  pixels: 

A!  =  (I(Xj  +  1, 17  -  1)  +  21  (X i  +  1,  17)  +  I{XT  +  1,  V/  +  1)) 

~(I(Xi  -  1, 17  -  1)  +  2 1(XT  -  1, 17)  +  I(Xi  -  1, 17  +  l)) 

A2  =  (I(X!  -  1, 17  +  1)  +  21  (X i,  17  +  1)  +  I(Xi  +  1,  17  +  1)) 

-(/(A7  -  1, 17  -  1)  +  21  (Xj,  17  -  1)  +  I(Xj  +  1,  Yj  -  1)). 
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Fig.  10.3.  Contours  of  image  of  Fig.  10.1  obtained  using  Roberts  (left)  and  Sobel 
(right)  operators 


Then,  the  approximated  magnitude,  or  norm,  of  gradient  G(A/,Yj)  can  be 
evaluated  using  one  of  the  following  two  expressions: 


G(XI,YI)  =  y/ Al  +  A* 
G(XI,YI)  =  |ZW|  +  |ZM, 

and  direction  9(Xj,Yj)  with  the  relationship 


6(XI,YI)  =  Atan2  (A2,A1). 

Figure  10.3  shows  the  images  obtained  from  that  of  Fig.  10.1  by  applying 
the  gradient  operators  of  Sobel  and  Roberts  and  binarization;  the  thresholds 
have  been  set  to  l  =  0.02  and  l  =  0.0146,  respectively. 

An  alternative  edge  detection  method  is  based  on  the  Laplacian  operator, 
which  requires  the  computation  of  the  second  derivatives  of  function  /(A/,  Yj) 
along  two  orthogonal  directions.  Also  in  this  case,  suitable  operators  are  used 
to  discretize  the  computation  of  derivatives.  One  of  the  most  common  approx¬ 
imations  is  the  following: 


L(Xl7  F/)  =  I(Xl7  Yj)  -  *  (I(Xt,  Yj  +  1)  +  I(Xj,  Yj  -  1) 

+  /(A/  +  1,  Yj)  +  /(A/  -  1,1/)) . 

In  this  case,  the  pixels  of  the  contour  are  those  where  the  Laplacian  is  lower 
than  a  threshold.  The  reason  is  that  the  Laplacian  is  null  at  the  points  of 
maximum  magnitude  of  the  gradient.  The  Laplacian,  unlike  the  gradient,  does 
not  provide  directional  information;  moreover,  being  based  on  the  calculation 
of  second  derivatives,  it  is  more  sensitive  to  noise  than  the  gradient. 
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10.2.2  Image  Interpretation 

Image  interpretation  is  the  process  of  calculating  the  image  feature  parameters 
from  the  segments,  whether  they  are  represented  in  terms  of  boundaries  or  in 
terms  of  regions. 

The  feature  parameters  used  in  visual  servoing  applications  sometimes  re¬ 
quire  the  computation  of  the  so-called  moments.  These  parameters  are  defined 
on  a  region  1Z  of  the  image  and  can  be  used  to  characterize  the  position,  ori¬ 
entation  and  shape  of  the  two-dimensional  object  corresponding  to  the  region 
itself. 

The  general  definition  of  moment  niij  of  a  region  TZ  of  a  framestore,  with 
i,j  =  0, 1, 2, . . .,  is  the  following: 

mij=  E  nXi^XiYf. 

AjTjgr 

In  the  case  of  binary  images,  by  assuming  the  light  intensity  equal  to  one  for 
all  the  points  of  region  TZ ,  and  equal  to  zero  for  all  the  points  not  belonging 
to  7 Z,  the  following  simplified  definition  of  moment  is  obtained: 

mij=  E  XM-  (10.1) 

In  view  of  this  definition,  moment  mo,o  coincides  with  the  area  of  the  region, 
computed  in  terms  of  the  total  number  of  pixels  of  region  7 Z. 

The  quantities 

mi  o  _  mo i 

x  ==  - - —  y  =  - — 

m0,o  m0,0 

define  the  coordinates  of  the  so-called  centroid  of  the  region.  These  coordinates 
can  be  used  to  detect  uniquely  the  position  of  region  TZ  on  the  image  plane. 

Using  an  analogy  from  mechanics,  region  TZ  can  be  seen  as  a  two- 
dimensional  rigid  body  of  density  equal  to  light  intensity.  Hence,  moment 
m o,o  corresponds  to  the  mass  of  the  body  and  the  centroid  corresponds  to  the 
centre  of  mass. 

The  value  of  moment  ml  ]  in  (10.1)  depends  on  the  position  of  region 
1Z  in  the  image  plane.  Therefore,  the  so-called  central  moments  are  often 
considered,  defined  as 

=  E  (Xi  ~  *Y(Yi  “  y)3’ 

x  ,,r,eR 

which  are  invariant  with  respect  to  translation. 

According  to  the  mechanical  analogy,  it  is  easy  to  recognize  that  the  central 
moments  of  second  order  /i2,o  and  /io,2  have  the  meaning  of  inertia  moments 
with  respect  to  axes  Xi  and  Yj  respectively,  while  /upi  is  an  inertia  product, 
and  the  matrix 

M2,o  ^1,1] 
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Fig.  10.4.  Region  of  a  binary  image  and  some  feature  parameters 


has  the  meaning  of  inertia  tensor  relative  to  the  centre  of  mass.  The  eigen¬ 
values  of  matrix  X  define  the  principal  moments  of  inertia,  termed  principal 
moments  of  the  region  and  the  corresponding  eigenvectors  define  the  principal 
axes  of  inertia,  termed  principal  axes  of  the  region. 

If  region  7 Z  is  asymmetric,  the  principal  moments  of  X  are  different  and  it 
is  possible  to  characterize  the  orientation  of  TZ  in  terms  of  the  angle  a  between 
the  principal  axis  corresponding  to  the  maximum  moment  and  axis  X.  This 
angle  can  be  computed  with  the  equation  (see  Problem  10.1) 


a  = 


2/41,1 

M  2,0  —  /*o, 2 


(10.2) 


As  an  example,  in  Fig.  10.4,  the  region  of  a  binary  image  is  shown;  centroid 
C,  the  principal  axes,  and  the  angle  a  are  evidenced. 

Notice  that  the  moments  and  the  corresponding  parameters  can  also  be 
computed  from  the  boundaries  of  the  objects;  moreover,  these  quantities  are 
especially  useful  to  characterize  objects  of  generic  form.  Often,  however,  the 
objects  present  in  the  scene,  especially  those  manufactured,  have  geometric 
characteristics  which  are  useful  to  take  into  account  for  image  interpretation. 

For  example,  many  objects  have  edges  that,  in  the  image  plane,  correspond 
to  the  intersection  of  linear  parts  of  contour  or  to  contour  points  of  high 
curvature.  The  coordinates  of  these  points  on  the  image  plane  can  be  detected 
using  algorithms  robust  against  the  noise,  and  therefore  can  be  used  as  feature 
parameters  of  the  image.  They  are  usually  termed  feature  points. 

In  other  cases,  it  is  possible  to  identify  true  geometric  primitives  such 
as  lines  or  line  segments,  which  are  projections  of  linear  edges  or  solids  of 
revolution  (cones,  cylinders),  or  ellipses,  obtained  as  projections  of  circles  or 
spheres.  These  primitives  can  be  characterized  on  the  image  plane  in  terms  of  a 
minimum  set  of  parameters.  For  example,  a  line  segment  can  be  characterized 
by  the  coordinates  of  its  endpoints,  or  alternatively,  by  the  coordinates  of  its 
midpoint  (centroid),  its  length  (moment  m o,o)  and  its  orientation  (angle  a);  in 
both  cases,  the  characterization  of  the  line  segment  requires  four  parameters. 
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Fig.  10.5.  Reference  frames  for  an  eye-in-hand  camera 


10.3  Pose  Estimation 

Visual  servoing  is  based  on  the  mapping  between  the  feature  parameters  of  an 
object  measured  in  the  image  plane  of  the  camera  and  the  operational  space 
variables  defining  the  relative  pose  of  the  object  with  respect  to  the  camera 
or,  equivalently,  of  the  camera  with  respect  to  the  object.  Often,  it  is  sufficient 
to  derive  a  differential  mapping  in  terms  of  velocity.  As  for  the  computation 
of  the  inverse  kinematics  of  a  manipulator,  the  differential  problem  is  easier 
to  solve  because  the  velocity  mapping  is  linear;  moreover,  the  solution  to 
the  differential  problem  can  be  used  to  compute  the  pose  by  using  numerical 
integration  algorithms. 

The  set  of  feature  parameters  of  an  image  defines  a  (fc  x  1)  vector  s,  termed 
feature  vector.  In  the  following,  to  simplify  notation,  normalized  coordinates 
(X,  Y)  defined  in  (5.44)  will  be  used  in  place  of  pixel  coordinates  (A/,  Yj)  to 
define  the  feature  vector.  Since  only  pixel  coordinates  can  be  directly  mea¬ 
sured,  the  normalized  coordinates  should  be  computed  from  pixel  coordinates 
using  the  inverse  of  mapping  (5.45),  provided  that  the  intrinsic  parameters  of 
the  camera  are  known. 

The  feature  vector  a  of  a  point  is  defined  as 

(10.3) 

while 


denotes  its  representation  in  homogeneous  coordinates. 
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10.3.1  Analytical  Solution 


Consider  a  camera,  for  example  an  eye-in-hand  camera,  and  a  reference  frame 
Oc-xcyczc  attached  to  the  camera;  consider  also  a  reference  frame  O0-x0y0z0 
attached  to  the  object,  supposed  to  be  rigid,  and  let  Tca  be  the  homogeneous 
transformation  matrix  corresponding  to  the  relative  pose  of  the  object  with 
respect  to  the  camera,  defined  as 


T 


C 

O 


K  °c,o' 
,0T  1  . 


(10.4) 


with  occ  0  =  oj  —  o°,  where  occ  is  the  position  vector  of  the  origin  of  the  camera 
frame  with  respect  to  the  base  frame,  expressed  in  camera  frame,  ocQ  is  the 
position  vector  of  the  origin  of  the  object  frame  with  respect  to  the  base  frame, 
expressed  in  the  camera  frame,  and  Rca  is  the  rotation  matrix  of  the  object 
frame  with  respect  to  the  camera  frame  (Fig.  10.5). 

The  problem  to  solve  is  that  of  computing  the  elements  of  matrix  Tca  from 
the  measurements  of  object  feature  parameters  in  the  camera  image  plane.  To 
this  end,  consider  n  points  of  the  object  and  let  r°0  i  =  —  o°,  i  =  1, . . . ,  n, 

denote  the  corresponding  position  vectors  with  respect  to  the  object  frame. 
These  quantities  are  assumed  to  be  known,  for  example,  from  a  CAD  model  of 
the  object.  The  projections  of  these  points  on  the  image  plane  have  coordinates 


and  define  the  feature  vector 


s  = 


s  1 


(10.5) 


The  homogeneous  coordinates  of  the  points  of  the  object  with  respect  to  the 
camera  frame  can  be  expressed  as 


n-»c-~o 

r  =  1  r  . 

'  0,1  O  0,1 


Therefore,  using  (5.44),  the  homogeneous  coordinates  of  the  projections  of 
these  points  on  the  image  plane  are  given  by 


A  &  =  nTcar° 


(10.6) 


with  Aj  >  0. 

Assume  that  n  correspondences  are  available,  namely  n  equations  of  the 
form  (10.6)  for  n  points  of  the  object,  whose  coordinates  are  known  both 
in  the  object  frame  and  in  the  image  plane.  These  correspondences  define 
a  system  of  equations  to  be  solved  for  the  unknown  elements  of  matrix  Tca. 
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Computing  the  solution  is  a  difficult  task  because,  depending  on  the  type 
and  on  the  number  of  correspondences,  multiple  solutions  may  exist.  This 
problem,  in  photogrammetry,  is  known  as  PnP  (Perspective-n-Point)  problem. 
In  particular,  it  can  be  shown  that: 

•  P3P  problem  has  four  solutions,  in  the  case  of  three  non-collinear  points. 

•  P4P  and  P5P  problems  each  have  at  least  two  solutions,  in  the  case  of 
non-coplanar  points,  while  the  solution  is  unique  in  the  case  of  at  least 
four  coplanar  points  and  no  triplets  of  collinear  points. 

•  PnP  problem,  with  n  >  6  non-coplanar  points,  has  only  one  solution. 

The  analytical  solution  to  PnP  problem  is  rather  laborious.  However,  the 
derivation  becomes  simpler  in  some  particular  cases  as,  for  example,  in  the 
case  of  coplanar  points. 

Without  loss  of  generality,  assume  that  the  plane  containing  the  points 
of  the  object  coincides  with  one  of  the  three  coordinate  planes  of  the  object 
frame,  for  instance,  with  the  plane  of  equation  za  =  0;  this  implies  that  all 
the  points  of  the  plane  have  the  third  coordinate  equal  to  zero.  Multiplying 
both  sides  of  (10.6)  by  the  skew-symmetric  matrix  5(5*),  the  product  on  the 
left-hand  side  is  zero,  leading  to  the  homogeneous  equation 

S(st)H[rX:i  rVii  1]T  =  0,  (10.7) 

where  r,,.4  and  ry *  are  the  two  non-null  components  of  vector  r°  i  and  H  is 
the  (3  x  3)  matrix 

H  =  [r1  r2  occ  o  ] ,  (10.8) 

r\  and  r2  being  the  first  and  the  second  column  of  rotation  matrix  Rca,  re¬ 
spectively. 

Vector  equation  (10.7),  defined  on  homogeneous  coordinates  of  points  be¬ 
longing  to  two  planes,  is  know  as  planar  homography,  this  denomination  is 
used  also  for  matrix  H . 

Notice  that  Eq.  (10.7)  is  linear  with  respect  to  H  and,  therefore,  can  be 
rewritten  in  the  form 

Ai{si)h  =  0, 

where  h  is  the  (9  x  1)  column  vector  obtained  by  staking  the  columns  of 
matrix  H,  while  A*  is  the  (3  x  9)  matrix 

Ai(si)  =  [rX;*5(5*)  rVtiS(si)  5(5*)].  (10.9) 

Since  the  rank  of  5(-)  is  at  most  2,  then  the  rank  of  matrix  A.j  is  also  at  most 
2;  therefore,  to  compute  h  (up  to  a  scale  factor),  it  is  necessary  to  consider  at 
least  4  equations  of  the  form  (10.9)  written  for  4  points  of  the  plane,  leading 
to  the  system  of  12  equations  with  9  unknowns 

^■l('Sl) 

h  =  A(s)h  =  0,  (10.10) 

-43(S3) 

A4(s4)_ 
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with  s  defined  in  (10.5). 

It  can  be  shown  that,  considering  a  set  of  four  points  with  no  triplets  of 
collinear  points,  matrix  A  has  rank  8  and  the  system  of  equations  in  (10.10) 
admits  a  non-null  solution  £/i,  defined  up  to  a  scaling  factor  C  (see  Prob¬ 
lem  10.2).  As  a  result,  matrix  QH  can  be  computed  up  to  a  scaling  factor  (. 
The  presented  derivation  is  general  and  can  be  applied  to  any  kind  of  planar 
homography  defined  by  an  equation  of  the  form  (10.7). 

In  view  of  (10.8),  it  is 

ri  =  Chi 
r2  =  Ch2 

occ,0  =  Ch3 

where  hi  denotes  the  i-tli  column  of  matrix  H.  The  absolute  value  of  constant 
C  can  be  computed  by  imposing  the  unit  norm  constraint  to  vectors  r  i  and 
r2: 

ICI  =  1  =  1 
M  IM  IIM’ 

while  the  sign  of  C  can  be  determined  by  choosing  the  solution  corresponding 
to  the  object  in  front  of  the  camera.  Finally,  the  third  column  r 3  of  matrix 
K  can  be  computed  as 

r3  =  n  x  r2. 

Notice  that,  because  of  the  noise  affecting  the  measurements  of  the  coor¬ 
dinates  in  the  image  plane,  the  results  of  this  derivation  are  affected  by  errors 
that  can  be  reduced  by  considering  a  number  n  >  4  of  correspondences  and 
computing  the  solution  ( h  to  the  3n  equations  in  (10.10),  up  to  a  scaling  fac¬ 
tor  C,  using  least-squares  techniques.  This,  however,  does  not  guarantee  that 
the  resulting  matrix  Q  =  [r  1  r2  r3  }  is  a  rotation  matrix. 

A  possible  solution  overcoming  this  problem  consists  of  computing  the 
rotation  matrix  ‘closest’  to  Q  with  respect  to  a  given  norm  such  as  the  matrix 
which  minimizes  the  Frobenius  norm1 

/  \V2 

I \K  -  Q\\f  =  (Tr  ((Rc0  -  Q)T  (Rca  -  Q))J  ,  (10.11) 

with  the  constraint  that  Rca  is  a  rotation  matrix.  The  problem  of  minimizing 
norm  (10.11)  is  equivalent  to  that  of  maximizing  the  trace  of  matrix  Rca  Q. 
It  can  be  shown  that  the  solution  to  this  problem  is 

'1  0  O' 

Rca  =  U  0  1  0  VT  (10.12) 

0  0  (T 

where  U  and  VT  are,  respectively,  the  left  and  right  orthogonal  matrices  of 
the  singular  value  decomposition  of  Q  =  USVT .  The  choice  er  =  det(C/VT) 
ensures  that  the  determinant  of  Rc0  is  equal  to  one  (see  Problem  10.3). 

1  The  Frobenius  norm  is  defined  in  Sect.  A. 4. 
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feature  points  image  plane 


-0.2 -0.1  0  0.1  0.2  -0.2 -0.1  0  0.1  0.2 
[m] 


Fig.  10.6.  Planar  object;  left:  object  frame  and  feature  points;  right:  feature  points 
projections  on  the  normalized  image  plane  of  a  camera  in  the  pose  of  Example  10.1 


Example  10.1 

Consider  a  planar  object  characterized  by  four  feature  points  shown  in  Fig.  10.6, 
where  the  object  frame  is  represented.  The  feature  points  Pi,  P2,  P3,  Pa  are  the 
vertices  of  a  square  with  side  length  l  =  0.1  m.  In  Fig.  10.6,  the  images  of  the 
projections  of  the  four  points  of  the  object  on  the  normalized  image  plane  of  the 
camera  are  shown  as  well,  under  the  assumption  that  the  relative  pose  of  the  object 
frame  with  respect  to  the  camera  frame  is  characterized  by  rotation  matrix 

'  0.7071  0  0.7071' 

Rl  =  Rz(0)Ry(Tv/4)Rx(0)  =  0  1  0 

.-0.7071  0  0.7071. 

and  position  vector  occ  o  —  [0  0  0.5  ]T  m.  The  normalized  coordinates  of  the 

four  points  of  the  object  can  be  computed  from  the  position  vectors  in  the  object 
frame  r°ol  =  [0  0  0]T  m,  r°o2  =  [0.1  0  0]T  m,  r°3  =  [0.1  0.1  0]T  m, 
ro,A  =  [0  0.1  0]T  m,  using  (10.6),  which  gives 


0.1647 

0 


0.1647] 
0.2329  54 


To  solve  the  inverse  problem,  namely  that  of  computing  matrix  TCCJ  from  the  co¬ 
ordinates  of  the  four  points  both  in  the  image  plane  and  in  the  object  frame,  it  is 
necessary  to  build  matrix  A(s)  from  four  matrices  Ai(si)  defined  in  (10.9).  It  is  easy 
to  verify  that  matrix  .A(s)  has  rank  8;  moreover,  a  non-null  solution  to  the  system 
of  equations  in  (10.10)  can  be  computed  using  the  singular  value  decomposition 

A  =  USVT 


and  coincides  with  the  last  column  of  matrix  V,  namely,  with  the  right  eigenvector 
corresponding  to  the  null  singular  value  of  A.  From  this  computation,  matrix 

'-0.4714  0  0 

c  H  =  0  -0.6667  0 

.  0.4714  0  -0.3333. 
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can  be  obtained.  Normalization  of  the  first  column  yields  |£|  =  1.5.  It  is  possible  to 
verify  that,  with  the  choice  £  =  —1.5,  the  exact  solution  for  o\ l  0  is  obtained;  more¬ 
over,  matrix  Q  coincides  numerically  with  the  sought  rotation  matrix  R%,  without 
using  any  kind  of  approximate  solution.  This  result  was  expected,  due  to  the  absence 
of  measurement  noise  affecting  image  plane  coordinates  s;. 


The  above  derivation  is  a  particular  case  of  a  method,  termed  direct  linear 
transformation,  which  is  aimed  at  computing  the  elements  of  matrix  Tc0  by 
solving  a  system  of  linear  equations  obtained  using  n  correspondences  relative 
to  points  in  generic  configuration.  In  detail,  from  equalities 

S(si)[Rc0  ol0]r°0ti  =  0f  (10.13) 

which  coincide  with  (10.7)  in  the  case  that  points  r°oi  belong  to  plane  zo  =  0, 
two  independent  linear  equations  with  12  unknowns  are  obtained,  taking  into 
account  that  matrix  S'(-)  is,  at  most,  of  rank  2.  Therefore,  n  correspondences 
produce  2 n  equations. 

It  can  be  shown  that,  considering  a  set  of  6  points  not  all  coplanar,  the 
matrix  of  coefficients  of  the  corresponding  system  of  12  equations  with  12 
unknowns  has  rank  11;  therefore,  the  solution  is  defined  up  to  a  scaling  factor. 
Once  this  solution  has  been  computed,  the  elements  of  rotation  matrix  Rca 
and  of  vector  occ  0  can  be  obtained  using  a  derivation  similar  to  that  presented 
above.  Notice  that,  in  practical  applications,  due  to  the  presence  of  noise,  the 
system  of  equations  has  rank  12  and  admits  only  the  null  solution.  In  this 
case,  it  is  necessary  to  consider  a  number  n  >  6  of  correspondences  and  to 
compute  the  solution  of  the  resulting  system  of  equations,  defined  up  to  a 
scaling  factor,  using  least-squares  techniques. 

In  conclusion,  the  presented  method  permits  the  computation  of  matrix 
Tc0,  characterizing  the  relative  pose  of  the  object  frame  with  respect  to  the 
camera  frame,  from  the  projections  of  n  points  of  the  object  on  the  camera 
image  plane.  To  this  end,  it  is  necessary  to  know  the  position  of  these  points 
with  respect  to  the  object  frame,  and  thus  the  object  geometry,  besides  the 
camera  intrinsic  parameters .  The  latter  are  required  for  computing  normalized 
coordinates  Si  from  pixel  coordinates. 

Notice  that,  if  it  is  required  to  compute  the  object  pose  with  respect  to  the 
base  frame  (as  usually  happens  in  the  case  of  eye-to-hand  cameras)  or  to  the 
end-effector  frame  (as  usually  happens  in  the  case  of  eye-in-hand  cameras), 
then  it  is  also  necessary  to  know  the  camera  extrinsic  parameters .  In  fact,  in 
the  first  case,  it  is 

Tb0  =  TbcTc0,  (10.14) 

where  the  elements  of  matrix  Tbc  represent  the  extrinsic  parameters  of  an 
eye-to-hand  camera;  on  the  other  hand,  in  the  case  of  eye-in-hand  camera,  it 
is 

Te0  =  T\T%,  (10.15) 
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where  the  extrinsic  parameters  matrix  Tec  characterize  the  pose  of  the  camera 
with  respect  to  the  end-effector  frame. 


10.3.2  Interaction  Matrix 


If  the  object  is  in  motion  with  respect  to  the  camera,  the  feature  vector  s  is, 
in  general,  time-varying.  Therefore,  it  is  possible  to  define  a  (k  x  1)  velocity 
vector  in  the  image  plane  s. 

The  motion  of  the  object  with  respect  to  the  camera  is  characterized  by 
the  relative  velocity 


v 


C 

c,o 


(10.16) 


where  occ  0  is  the  time  derivative  of  vector  occo  =  R^(o0  —  oc),  representing 
the  relative  position  of  the  origin  of  the  object  frame  with  respect  to  the  origin 
of  the  camera  frame,  while  u>0  and  ujc  are  the  angular  velocities  of  the  object 
frame  and  camera  frame,  respectively. 

The  equation  relating  s  to  0  is 


S  =  Ja{s,T%)v 


C 

c,o> 


(10.17) 


where  Js  is  a  (k  x  6)  matrix  termed  image  Jacobian.  This  equation  is  linear 
but  J s  depends,  in  general,  on  the  current  value  of  the  feature  vector  s  and 
on  the  relative  pose  of  the  object  with  respect  to  the  camera  Tc0. 

It  is  useful  to  consider  also  the  mapping  between  the  image  plane  velocity 
s,  the  absolute  velocity  of  the  camera  frame 


v 


C 

C 


'  RTcbc  - 
.RcWcr. 


and  the  absolute  velocity  of  the  object  frame 


v 


c 

o 


RTcb0- 

_RTcUo_ 


To  this  end,  vector  occ  0  can  be  expressed  as 


b 


c 

c,o 


RT(o0  -  oc)  +  S(o°t0)RcWc, 


which  allows  equality  (10.16)  to  be  rewritten  in  the  compact  form 


v 


c 

c,o 


Vo  +  r(°c,o)Vci 


(10.18) 


r(-) 


-I  S(-) 

O  I 


with 
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Therefore,  Eq.  (10.17)  can  be  rewritten  in  the  form 

k  =  Jav%  +  Lav%,  (10.19) 

where  the  ( k  x  6)  matrix 

Ls  =  J  s(s,Tc0)r{occo)  (10.20) 

is  termed  interaction  matrix.  This  matrix,  in  view  of  (10.19),  defines  the  linear 
mapping  between  the  absolute  velocity  of  the  camera  vcc  and  the  corresponding 
image  plane  velocity  s,  in  the  case  that  the  object  is  fixed  with  respect  to  the 
base  frame  (vca  =  0). 

The  analytical  expression  of  the  interaction  matrix  is,  in  general,  simpler 
than  that  of  the  image  Jacobian.  The  latter  can  be  computed  from  the  inter¬ 
action  matrix  using  the  equation 

Js(s,Tc0)  =  Lsr(-ocCt0),  (10.21) 

obtained  from  (10.20),  with  T,_1(o“  Q)  =  r(—occ  Q).  In  the  following,  examples 
of  computation  of  interaction  matrix  and  image  Jacobian  for  some  of  the  most 
common  cases  in  applications  are  provided. 


Interaction  matrix  of  a  point 


Consider  a  point  P  of  the  object  characterized,  with  respect  to  the  camera 
frame,  by  the  vector  of  coordinates 

rcc=RTc{p-oc ),  (10.22) 


where  p  is  the  position  of  point  P  with  respect  to  the  base  frame.  Choose 
vector  s  of  normalized  coordinates  (10.3)  as  the  feature  vector  of  the  point. 
In  view  of  (5.44),  the  following  expression  holds: 

«  =  (10-23) 


with 


S(rc) 


1 

Xc' 

'X' 

-2c 

.Vc. 

_Y  _ 

(10.24) 


and  rcc  =  [xc  yc 
ing  (10.24)  yields 


Computing  the  time  derivative  of  (10.23)  and  us- 


s  = 


ds{rcc )  -  a 

dr %  c 


(10.25) 


with 


ds{rl)  1 

'1  0 

Xc/ Zc 

l 

"1  0  -X' 

drcc  zc 

.0  1 

-Vc/Zc  . 

1  u 

1  ^ 

.o  i  -y_ 
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To  compute  the  interaction  matrix,  vector  rcc  can  be  obtained  from  the  time 
derivative  of  (10.22)  under  the  assumption  that  p  is  constant: 

rcc  =  -R^dc  +  S(rcc)R^c  =  [-I  S(rcc)]vcc.  (10.26) 


Combining  Eqs.  (10.25),  (10.26),  the  following  expression  of  interaction  matrix 
of  a  point  can  be  found: 


Ls{s,  zc) 


X 

XY 

-( 1  +  X 2)  Y  ' 

1  +  Y2 

-XY  -X 

zc  zc 


(10.27) 


revealing  that  this  matrix  depends  on  the  components  of  vector  s  and  the  sole 
component  zc  of  vector  rcc. 

The  image  Jacobian  of  a  point  can  be  computed  using  (10.21),  (10.27)  and 
has  the  expression 


Js{s,Tc0)  =  - 

Zr. 


1  0  -X  -r^yX 
0  1  -Y  -(rc0,z+rc0,yY) 


rc  X  — rc 

'  o,x  o,y 

rY  r* 


where  ,  r£  ,  r„z  are  the  components  of  vector  —  occo  =  Rc0r°,  r° 

being  the  constant  vector  expressing  the  position  of  point  P  with  respect  to 
the  object  frame. 


Interaction  matrix  of  a  set  of  points 

The  interaction  matrix  of  a  set  of  n  points  of  the  object  P\ , . . .  Pn  can  be  built 
by  considering  the  (2 n  x  1)  feature  vector  (10.5).  If  LSi(si,zc ,j)  denotes  the 
interaction  matrix  corresponding  to  point  P*,  then  the  interaction  matrix  of 
the  set  of  points  will  be  the  (2 n  x  6)  matrix 


-^s(sj  zc) 


^si  (^1?  zc>1) 

(^717  -2c,7l) 


with  zc  =  [zCt i . . .  zc,n]T . 

The  image  Jacobian  of  a  set  of  points  can  be  easily  computed  from  the 
interaction  matrix,  using  (10.21). 


Interaction  matrix  of  a  line  segment 

A  line  segment  is  the  part  of  the  line  connecting  two  points  Pi  and  P2.  The 
projection  on  the  image  plane  is  still  a  line  segment  that  can  be  characterized 
in  terms  of  the  middle  point  coordinates  x,  y,  the  length  L ,  and  the  angle  a 


10.3  Pose  Estimation  427 


formed  by  the  line  with  respect  to  axis  X.  Therefore,  the  feature  vector  can 
be  defined  as 


"  X  " 

(Xi  +  A2)/2 

y 

(Yi  +  Y2)/ 2 

L 

V AX 2  +  AY2 

. a _ 

.tan  ~1(AY/AX)_ 

a(s1,s2)  (10.28) 


with  AX  =  Xi  —  Xi,  AY  =  Y%  —  Yi  and  s,  =  [  X,  Yi  ]T ,  i  =  1, 2.  Computing 
the  time  derivative  of  this  equation  yields 


ds 


s  = 


dsi 
ds 


«i 


ds 

ds- 


-s2 


ds 


a  LSl{Si,Zc^l)  + 

OS i  OS2 


LS2{s2i  zc,2) 


V 


c 

c’ 


where  LSi  is  the  interaction  matrix  of  point  P,;,  under  the  assumption  that  the 
line  segment  is  fixed  with  respect  to  the  base  frame.  Therefore,  the  interaction 
matrix  of  a  line  segment  is 

ds  ds 

Ls(s,Zc)  =  LSl(sUZCtl)  +  —  LS2(s2,  ZCt2), 

ds 1  ds 2 

with 


r  1/2 

0 

r  1/2 

0  ' 

ds 

0 

1/2 

ds 

0 

1/2 

ds\ 

-AX/L 

-AY/L 

ds2 

AX/L 

AY/L 

.  AY/L2 

-AX/L2. 

.-AY/L2 

AX/L2. 

Notice  that  vectors  Si  and  s2  can  be  computed  as  functions  of  parameters  x, 
y,  L ,  a,  using  (10.28).  Therefore,  the  interaction  matrix  can  be  expressed  as 
a  function  of  the  feature  vector  s  =  [x  y  L  a]T,  besides  the  components 
zC)  1  and  zCt2  of  the  endpoints  Pi  and  P2  of  the  line  segment. 

The  image  Jacobian  of  a  line  segment  can  be  easily  computed  from  the 
interaction  matrix  using  (10.21). 


10.3.3  Algorithmic  Solution 

The  interaction  matrix  Ls  is,  in  general,  a  matrix  of  dimension  (fc  x  m), 
where  k  is  equal  to  the  number  of  feature  parameters  of  the  image  and  m  is 
the  dimension  of  velocity  vector  vcc.  Usually  in  =  6,  but  it  may  happen  that 
m  <  6,  when  the  relative  motion  of  the  object  with  respect  to  the  camera  is 
constrained. 

The  image  Jacobian  J s  is  also  of  dimension  ( k  x  m),  being  related  to  Ls 
by  mapping  (10.21).  Since  this  mapping  is  invertible,  the  rank  of  J s  coincides 
with  that  of  Ls. 
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In  the  case  that  Ls  is  full  rank,  by  using  (10.17),  it  is  possible  to  compute 
vcc  0  from  s. 

In  particular,  if  k  =  m,  the  velocity  vcc  0  can  be  obtained  using  the  expres¬ 
sion 

vcc,o  =  nolJLj's,  (10.29) 

which  requires  the  computation  of  the  inverse  of  the  interaction  matrix. 

In  the  case  k  >  m,  the  interaction  matrix  has  more  rows  than  columns 
and  Eq.  (10.17)  can  be  solved  using  a  least-squares  technique,  whose  solution 
can  be  written  in  the  form 

vl0  =  r{ol0){LTsLs)-1LTss ,  (10.30) 

where  (l/fis)_1  is  the  left  pseudo-inverse  of  Ls.  This  situation  is  rather 
frequent  in  applications,  because  it  permits  using  interaction  matrices  with 
good  condition  numbers. 

Finally,  in  the  case  k  <  m,  the  interaction  matrix  has  more  columns  than 
rows  and  Eq.  (10.17)  admits  infinite  solutions.  This  implies  that  the  number 
of  parameters  of  the  observed  image  is  not  sufficient  to  determine  uniquely 
the  relative  motion  of  the  object  with  respect  to  the  camera.  Hence,  there 
exist  relative  motions  of  the  object  with  respect  to  the  camera  (or  vice  versa) 
that  do  not  produce  variations  of  the  image  feature  parameters.  The  velocities 
associated  with  these  relative  motions  belong  to  the  null  subspace  of  Js ,  which 
has  the  same  dimension  of  the  null  subspace  of  Ls.  If  the  problem  is  that  of 
computing  uniquely  the  relative  pose  of  the  object  with  respect  to  the  camera 
from  feature  parameters  in  the  image  plane,  this  case  has  no  interest. 


Example  10.2 

The  interaction  matrix  of  a  point  P  is  a  matrix  with  more  columns  than  rows  of 
dimension  (2  x  6)  and  rank  2;  therefore,  the  null  subspace  has  dimension  4.  It  can  be 
seen  immediately  that  this  subspace  contains  the  velocity  of  the  camera  translational 
motion  along  the  visual  ray  projecting  point  P  on  the  image  plane,  proportional  to 
vector 

V!  =  [X  Y  1  0  0  0]T, 

as  well  as  the  velocity  of  the  camera  rotational  motion  about  this  visual  ray,  pro¬ 
portional  to  vector 

«2  =  [0  0  0  X  Y  1]T . 

Vectors  v\  and  i>2  are  independent  and  belong  to  a  base  of  the  null  subspace  of 
Ls.  The  remaining  base  vectors  are  not  easy  to  find  geometrically,  but  can  be  easily 
computed  analytically. 


The  pose  estimation  problem  may  be  cast  in  a  form  analogous  to  that 
of  inverse  kinematics  algorithms  for  robot  manipulators.  To  this  end,  it  is 
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necessary  to  represent  the  relative  pose  of  the  object  with  respect  to  the 
camera  using  a  minimum  number  of  coordinates,  in  terms  of  the  (to  x  1) 
vector 

(10.31) 

where  occ  0  characterizes  the  position  of  the  origin  of  the  object  frame  with 
respect  to  the  camera  frame  and  <pc  0  characterizes  the  relative  orientation.  If 
Euler  angles  are  used  to  represent  orientation,  then  <j>c  0  is  the  vector  of  the 
angles  extracted  from  rotation  matrix  Rc0  and  the  mapping  between  vcc  0  and 
xCi0  is  expressed  by 

vc,o  =  q  T((J)  )  ^c,o  =  T  A{cj)C0)xCt0.  (10.32) 


Example  10.3 

Consider  a  camera  mounted  on  the  end-effector  of  the  SCARA  manipulator  of 
Fig.  2.36.  Choose  the  camera  frame  parallel  to  the  end-effector  frame,  with  axis 
zc  pointing  downward.  Assume  that  the  camera  observes  a  fixed  planar  object,  par¬ 
allel  to  the  image  plane,  and  that  axis  z0  of  the  object  frame  is  parallel  to  axis  zc 
and  points  downward. 

The  geometry  of  the  problem  suggests  that  the  relative  position  of  the  object 
with  respect  to  the  camera  can  be  represented  by  a  vector  occ  o,  whereas  the  relative 
orientation  can  be  defined  by  the  angle  a  between  the  object  frame  and  the  camera 
frame  about  axis  zc.  Therefore,  m  =  4  and 

(10.33) 

Moreover,  the  time  derivative  a  coincides  with  the  component  of  along  zc,  and 
this  is  the  sole  non-null  component  of  the  angular  velocity  of  the  relative  motion  of 
the  object  frame  with  respect  to  the  camera  frame.  Hence,  in  (10.32),  Ta(<?5>co)  is 
the  (4  x  4)  identity  matrix. 

Equation  (10.17)  can  be  rewritten  in  the  form 

s  =  J As  (s,  xc  o)xc  o^  (10.34) 

where  the  matrix 

JAs  (a,  xc,0)  =  Lsr{—occo)TA{<f>co)  (10.35) 

has  a  meaning  analogous  to  that  of  the  analytical  Jacobian  of  a  manipulator. 
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r 


Fig.  10.7.  Pose  estimation  algorithm  based  on  the  inverse  of  image  Jacobian 


Equation  (10.34)  is  the  starting  point  of  a  numeric  integration  algorithm 
for  the  computation  of  xCt0,  similar  to  inverse  kinematics  algorithms.  Let  xCt0 
denote  the  current  estimate  of  vector  xc  0  and  let 

S  =  s(xCto) 

be  the  corresponding  vector  of  image  feature  parameters  computed  from  the 
pose  specified  by  xc  o;  the  objective  of  this  algorithm  is  the  minimization  of 
the  error 

es  =  s  —  s.  (10.36) 

Notice  that,  for  the  purpose  of  numerical  integration,  vector  s  is  constant  while 
the  current  estimate  s  depends  on  the  current  integration  time.  Therefore, 
computing  the  time  derivative  of  (10.36)  yields 

Gs  —  S  J J4s  (s,  xc^0^xc^0.  (10.37) 

Assumed  that  matrix  J  as  is  square  and  nonsingular,  the  choice 

Xq^o  —  *J As  xc^0^J^ SGS  (10.38) 

leads  to  the  equivalent  linear  system 

gs  +  K  SGS  =  0.  (10.39) 

Therefore,  if  Ks  is  a  positive  definite  matrix  (usually  diagonal),  system  (10.39) 
is  asymptotically  stable  and  the  error  tends  to  zero  with  a  convergence  speed 
that  depends  on  the  eigenvalues  of  matrix  Ks.  The  convergence  to  zero  of 
error  es  ensures  the  asymptotic  convergence  of  the  estimate  xCtCI  to  the  true 
value  xC'0. 

The  block  scheme  of  the  pose  estimation  algorithm  is  shown  in  Fig.  10.7, 
where  s(-)  denotes  the  function  computing  the  feature  vector  of  the  ‘virtual’ 
image  corresponding  to  the  current  estimate  x0>0  of  the  object  pose  with  re¬ 
spect  to  the  camera.  This  algorithm  can  be  used  as  an  alternative  to  the 
analytical  methods  for  pose  estimation  illustrated  in  Sect.  10.3.1.  Obviously, 
the  convergence  properties  depend  on  the  choice  of  the  image  feature  parame¬ 
ters  and  on  the  initial  value  of  estimate  £cCjO(0),  which  may  produce  instability 
problems  related  to  the  singularities  of  matrix  J ab- 
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Notice  that,  in  view  of  (10.35),  the  singularities  of  matrix  J  as  are  both 
the  representation  singularities  of  the  orientation  and  those  of  the  interaction 
matrix.  The  most  critical  singularities  are  those  of  the  interaction  matrix, 
since  they  depend  on  the  choice  of  the  image  feature  parameters. 

To  separate  the  effects  of  the  two  types  of  singularities,  it  is  convenient  to 
compute  (10.38)  in  two  steps,  evaluating  first 


Ko  =  r(°c,0)I,~s1Ksea, 

(10.40) 

and  then 

xc,o  =  T A  ((f>c,o)vc,o- 

(10.41) 

Assumed  to  work  far  from  representation  singularities,  the  problem  of 
singularities  of  Ls  can  be  overcome  by  using  a  number  k  of  feature  parameters 
greater  than  the  minimum  required  m.  This  choice  also  allows  a  reduction  of 
the  effects  of  measurement  noise.  The  resulting  estimation  algorithm  requires 

the  use  of  the  left  pseudo-inverse  of  Ls  in  place  of  the  inverse, 

namely 

v%  =  r{ol0){LTsLs)-1LTsKses 

(10.42) 

in  place  of  (10.40).  The  convergence  of  error  (10.36)  can  be  shown  using  the 
direct  Lyapunov  method  based  on  the  positive  definite  function  2 

V (es)  =  K sea  >  0  Ves  ^  0. 

Computing  the  time  derivative  of  this  function,  and  using  (10.37),  (10.35), 
(10.41),  (10.42),  yields 

V  =  -eTKaLs(LTLs)~1LTKae8, 

which  is  negative  semi-definite  because  Af(L^)  ^  0,  L J  being  a  matrix  with 
more  columns  than  rows.  Therefore,  the  system  is  stable  but  not  asymptot¬ 
ically  stable.  This  implies  that  the  error  is  bounded,  but  in  some  cases  the 
algorithm  can  get  stuck  with  es  ^  0  and  Kses  €  AT(L^). 

Notice  that  the  pose  estimation  methods  based  on  inverse  Jacobian  are  as 
efficient  in  terms  of  accuracy,  speed  of  convergence  and  computational  load, 
as  the  initial  estimate  *o>o(0)  is  close  to  the  true  value  xC)0.  Therefore,  these 
methods  are  mainly  adopted  for  real-time  ‘visual  tracking’  applications,  where 
the  estimate  on  an  image  taken  at  time  i  is  computed  assuming  as  initial  value 
the  estimate  computed  on  the  image  taken  at  time  t  —  T,  T  being  the  sampling 
time  of  the  image  (multiple  of  the  sampling  time  of  the  numerical  integration 
algorithm) . 


See  Sect.  C.3  for  the  illustration  of  the  direct  Lyapunov  method. 


2 
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error  norm  image  plane 


[s] 


Fig.  10.8.  Time  history  of  the  norm  of  the  estimation  error  and  corresponding 
paths  of  the  feature  points  projections  on  image  plane 


Fig.  10.9.  Time  history  of  camera  pose  estimate 


Example  10.4 

Consider  the  object  of  Example  10.1.  Using  the  algorithmic  solution,  it  is  desired 
to  compute  the  relative  pose  of  the  object  frame,  with  respect  to  the  camera  frame, 
from  the  image  plane  coordinates  of  the  projections  of  the  four  feature  points  of  the 
object.  The  same  numerical  values  of  Example  10.1  are  used. 

Since  the  image  Jacobian  has  dimension  (6  x  8),  it  is  necessary  to  use  the 
algorithm  based  on  the  pseudo- inverse  of  the  interaction  matrix.  This  algorithm 
was  simulated  on  a  computer  by  adopting  the  Euler  numerical  integration  scheme 
with  an  integration  time  At  =  1ms,  matrix  gain  Ka  =  160/s,  and  initial  estimate 
®c,o  =  [0  0  1  0  tt/32  0]T. 

The  results  in  Fig.  10.8  show  that  the  norm  of  the  estimation  error  of  the  feature 
parameters  es  tends  to  zero  asymptotically  with  convergence  of  exponential  type; 
moreover,  due  to  the  fact  that  matrix  gain  K 3  was  chosen  diagonal  with  equal 
elements,  the  paths  of  the  projections  of  the  feature  points  on  the  image  plane 
(between  the  initial  positions  marked  with  crosses  and  the  final  positions  marked 
with  circles)  are  line  segments. 

The  corresponding  time  histories  of  the  components  of  vector  SCiD  for  position 
and  orientation  are  reported  in  Fig.  10.9,  together  with  the  time  histories  of  the 
components  of  the  true  value  xCt0  =  [0  0  0.5  0  7r/4  0]t  (represented  with 
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dashed  lines).  It  can  be  verified  that,  with  the  chosen  value  of  Ka ,  the  algorithm 
converges  to  the  true  value  in  about  0.03  s,  corresponding  to  30  iterations. 

The  time  histories  of  Fig.  10.9  can  be  interpreted  as  the  position  and  orientation 
trajectories  of  a  ‘virtual’  camera  in  motion  between  the  initial  pose  a;CjO(0)  and  the 
final  pose  xC}0. 

Notice  that,  for  the  purpose  of  pose  estimation,  the  illustrated  algorithm  may 
converge  also  in  the  case  that  only  three  feature  points  are  used.  In  fact,  in  this  case, 
J As  is  a  square  (6  x  6)  matrix  and  the  convergence  is  ensured  provided  that  this 
matrix  is  nonsingular.  However,  since  P3P  problem  has  four  solutions,  the  algorithm 
may  converge  to  a  solution  different  from  that  sought,  unless,  as  for  visual  tracking 
applications,  the  initial  estimate  xc,o(0)  is  close  enough  to  the  true  value  xCj0. 


10.4  Stereo  Vision 

The  bidimensional  image  provided  by  a  camera  does  not  give  any  explicit 
information  on  depth,  namely,  the  distance  of  the  observed  object  from  the 
camera.  This  information  can  be  recovered  in  an  indirect  way  from  the  geo¬ 
metric  model  of  the  object,  assumed  to  be  known. 

On  the  other  hand,  the  depth  of  a  point  can  be  directly  computed  in 
the  case  that  two  images  of  the  same  scene  are  available,  taken  from  two 
different  points  of  view.  The  two  images  can  be  obtained  using  two  cameras, 
or  sequentially,  using  a  moving  camera.  These  cases  are  referred  to  as  stereo 
vision. 

In  the  framework  of  stereo  vision,  two  fundamental  problems  can  be  de¬ 
vised.  The  first  is  the  correspondence  problem,  which  consists  of  the  identifica¬ 
tion  of  the  points  of  the  two  images  that  are  projections  of  the  same  point  of 
the  scene.  These  points  are  termed  conjugate  or  corresponding .  This  problem 
is  not  easy  to  solve,  and  the  solution  is  based  on  the  existence  of  geometric 
constraints  between  two  images  of  the  same  point,  besides  the  fact  that  some 
details  of  the  scene  appear  to  be  similar  in  the  two  images. 

The  second  problem,  illustrated  below  in  some  fundamental  aspects,  is 
that  of  3D  reconstruction  which,  in  general,  consists  of  the  computation  of 
the  relative  pose  of  the  cameras  (calibrated  and  not)  and  thus,  starting  from 
this  pose,  the  position  in  the  3D  space  of  the  points  of  the  observed  object. 

10.4.1  Epipolar  Geometry 

Assume  that  two  cameras  are  available,  with  respective  reference  frames,  de¬ 
noted  as  1  and  2.  Moreover,  let  o\  2  denote  the  position  vector  and  R\  the 
rotation  matrix  of  Frame  2  with  respect  to  Frame  1  and  let  T\  be  the  corre¬ 
sponding  homogeneous  transformation  matrix.  The  coordinates  of  a  point  P 
expressed  in  the  two  frames  are  related  by  equation 

p1  =  o}2  +  Rip2. 


(10.43) 
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P 


Let  and  S2  be  the  coordinates  of  the  projections  of  P  on  the  image  planes 
of  the  cameras;  in  view  of  (5.44)  it  is 

Xisi  =  npi=pi,  *  =  1,2.  (10.44) 

Substituting  (10.44)  into  (10.43)  yields 

A1S1  =  o\2  +  A2-R2s2-  (10.45) 

Premultiplying  both  sides  of  (10.45)  by  S(o^2)  gives 

AiS(o1i2)5i  =  \2S{ol2)R}~s2. 

Hence,  premultiplying  both  sides  of  the  above  equation  by  ,  the  following 
equality  is  obtained 

A2sf  S(o{i2)R%82  =  0, 

which  has  to  be  satisfied  for  any  value  of  scalar  A2.  Therefore,  this  equality  is 
equivalent  to  the  so-called  epipolar  constraint  equation 

s^Es2  =  0,  (10.46) 

where  E  =  S(o\2)R}2  is  a  (3  x  3)  matrix  known  as  essential  matrix.  Equa¬ 
tion  (10.46)  expresses  in  analytical  form  the  geometric  constraint  existing 
between  the  projections  of  the  same  point  on  the  image  planes  of  the  two 
cameras. 

The  geometric  interpretation  of  the  epipolar  constraint  can  be  derived 
from  Fig.  10.10,  where  the  projections  of  a  point  P  on  the  image  planes  of 
the  two  cameras  are  reported  as  well  as  the  respective  optical  centers  Oi  and 
02.  Notice  that  points  0 1,  02  and  P  are  the  vertices  of  a  triangle  whose  sides 
0\P  and  02P  belong  to  the  visual  rays  projecting  point  P  into  the  points  of 
coordinates  Sj  and  s2  of  the  image  plane,  respectively.  These  rays  lay  along 
the  directions  of  vectors  si  and  R2s2  respectively,  expressed  with  respect  to 
Frame  1.  Line  segment  0\02,  termed  base  line,  is  represented  by  vector  o\  2. 
The  epipolar  constraint  (10.46)  corresponds  to  imposing  that  vectors  s-j ,  R\s2 
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and  o\  2  are  coplanar.  The  plane  containing  these  vectors  is  termed  epipolar 
plane. 

Notice  that  line  segment  0\0-2  belongs  to  the  visual  ray  projecting  point 
O2  on  the  image  plane  of  Camera  1  and,  at  the  same  time,  to  the  ray  projecting 
point  0\  on  the  image  plane  of  Camera  2.  These  projections,  of  coordinates 
ei  and  e.2,  respectively,  are  termed  epipoles.  Line  £1,  passing  through  the 
points  of  coordinates  Sj  and  elt  and  line  £ 2 ,  passing  through  the  points  of 
coordinates  S2  and  e2,  are  termed  epipolar  lines.  The  epipolar  lines  can  also 
be  obtained  as  the  intersection  of  the  epipolar  plane  with  the  image  planes  of 
the  two  cameras.  Notice  that,  by  varying  point  P ,  the  epipolar  plane  describes 
a  set  of  planes  about  the  base  line  and  the  epipoles  do  not  change. 

For  the  purpose  of  computing  the  correspondences,  the  epipolar  constraint 
can  be  exploited  to  reduce  the  complexity  of  the  problem  to  find  conjugate 
points.  In  fact,  if  Si  is  the  image  of  a  point  of  the  visual  ray  passing  through 
0\  and  the  point  of  coordinates  Si,  the  corresponding  conjugate  point  of 
the  image  plane  of  Camera  2  must  necessarily  belong  to  the  epipolar  line  £2, 
which  is  known  because  the  epipolar  plane  is  uniquely  defined  by  0\ ,  O2  and 
by  the  point  of  coordinates  Si .  Finding  correspondences,  therefore,  reduces 
to  searching  for  a  point  along  the  epipolar  line  and  not  on  the  whole  image 
plane. 

In  the  framework  of  3D  reconstruction,  different  scenarios  may  arise,  de¬ 
pending  on  the  type  of  information  which  is  available  a  priori. 

10.4.2  Triangulation 

In  the  case  that  both  intrinsic  and  extrinsic  parameters  of  the  two  cameras 
are  known,  the  reconstruction  problem  consists  of  computing  the  position  in 
the  scene  of  the  points  projected  on  the  two  image  planes  using  a  geometric 
method  known  as  triangulation.  This  method  allows  the  computation  of  co¬ 
ordinates  p  =  [px  Py  pz]T  of  a  point  P  with  respect  to  the  base  frame, 
starting  from  normalized  coordinates  s-i  =  [X\  Fj  }T  and  S2  =  [X2  ^2]T 

of  the  projections  of  P  on  the  image  planes  of  the  two  cameras.  Assume  that, 
for  simplicity,  the  base  frame  coincides  with  Frame  1,  then  p1  =  p,  o\  2  =  o 
and  R\  =  R. 

From  (10.44),  (10.45)  the  following  equalities  can  be  derived: 


p  =  A1S1  (10.47) 

p  =  o+X2Rs2,  (10.48) 

where  the  first  equality  is  the  parametric  equation  of  the  visual  ray  passing 
through  0\  and  the  point  of  coordinates  si,  while  the  second  represents  the 
parametric  equation  of  the  visual  ray  passing  through  O2  and  the  point  of 
coordinates  S2;  both  equations  are  expressed  in  the  base  frame. 

Therefore,  the  coordinates  of  point  P  at  the  intersection  of  the  two  visual 
rays  can  be  computed  by  solving  the  system  of  two  Eqs.  (10.47),  (10.48)  with 
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respect  to  p.  To  this  end,  from  (10.47),  by  computing  Ai  in  the  third  equation 
and  replacing  its  value  into  the  other  two  equations,  the  following  system  is 
obtained: 


1  0  -Xx 
0  1  —Y\ 


p  =  0. 


(10.49) 


Using  a  similar  derivation  for  the  equation  obtained  by  premultiplying  both 
sides  of  (10.48)  by  RT ,  the  following  system  is  obtained 


"  rT  —  X2r3  " 

Ox  ~  OzX 2 

r2  -  Y2r3  _ 

P  = 

Oy  —  0ZY2  _ 

(10.50) 


with  R  =  [n  r2  r3]  and  RT o  =  [ox  oy  oz]T .  Equations  (10.49), 

(10.50)  define  a  system  of  four  equations  and  three  unknowns,  which  is  linear 
with  respect  to  p.  Of  these  equations,  only  three  are  independent  in  the  ideal 
case  that  the  two  visual  rays  intersect  at  point  P.  In  practical  applications, 
because  of  noise,  these  equations  are  all  independent  and  the  system  has  no 
solution;  hence,  suitable  algorithms  based  on  least-squares  techniques  have  to 
be  adopted  to  compute  an  approximate  solution. 

Computation  of  p  can  be  greatly  simplified  in  the  case,  rather  frequent 
in  applications,  that  the  two  cameras  have  parallel  and  aligned  image  planes, 
with  R  =  I  and  RT o  =  [b  0  0]T,  b  >  0  being  the  distance  between  the 

origins  of  the  two  camera  frames.  This  implies  that  the  solution  to  the  system 
of  Eqs.  (10.49),  (10.50)  is 


Px  = 

Py  = 


Xxb 

xx-x2 

Y,b 

X1  X2 


Pz  = 


Y2b 

x±  X2 


(10.51) 

(10.52) 

(10.53) 


10.4.3  Absolute  Orientation 

In  the  case  of  a  calibrated  system  of  two  cameras  observing  a  rigid  object 
of  unknown  shape,  triangulation  can  be  used  to  compute  the  variation  of 
pose  of  the  object  or  of  the  system  of  cameras,  due  to  the  relative  motion 
of  the  object  with  respect  to  the  cameras.  This  problem,  known  as  absolute 
orientation,  requires  the  measurement  of  the  positions  of  the  projections  of  a 
certain  number  of  feature  points  of  the  object. 

If  the  stereo  camera  system  is  moving  and  the  object  is  fixed,  let  p1,...,pn 
denote  the  position  vectors  of  n  points  of  the  rigid  object  measured  at  time 
t  and  p\, ... , p'n  the  position  vectors  of  the  same  points  measured  at  time  t' 
using  triangulation.  These  vectors  are  all  referred  to  Frame  1  and,  under  the 
assumption  of  rigid  motion,  satisfy  the  equations 

Pi  =  °+  RP, 


i  =  1, . . . ,  n 


(10.54) 
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where  vector  o  and  rotation  matrix  R  define  the  position  and  orientation 
displacement  of  Frame  1  between  time  t  and  time  t' .  The  absolute  orientation 
problem  consists  of  the  computation  of  R  and  o  from  pi  and  p't. 

From  rigid  body  mechanics  it  is  known  that  this  problem  has  a  unique 
solution  in  the  case  of  three  non-collinear  points.  In  this  case,  nine  nonlinear 
equations  can  be  derived  from  (10.54),  in  terms  of  the  nine  independent  pa¬ 
rameters  which  characterize  o  and  R.  However,  since  the  points  are  obtained 
using  triangulation,  the  measurements  are  affected  by  error  and  the  system 
may  have  no  solution.  In  this  case,  it  is  convenient  to  consider  a  number  n  >  3 
of  points  and  compute  o  and  R  as  the  quantities  which  minimize  the  linear 
quadratic  function 

n 

YJ\Pi~  °~  Rp'if,  (10.55) 

with  the  constraint  that  R  is  a  rotation  matrix.  The  problem  of  computing  o 
can  be  separated  from  the  problem  of  computing  R  observing  that  the  value 
of  o  which  minimizes  function  (10.55)  is  (see  Problem  10.6) 

o  =  p  Rp'  (10.56) 

where  p  and  p'  are  the  centroids  of  the  set  of  points  {p,}  and  |p'},  defined 
as 

-t  n  i  n 

P=-X>.  p'  =  “51  Pi- 

i—l  i=l 

Hence  the  problem  becomes  that  of  computing  the  rotation  matrix  R  which 
minimizes  the  linear  quadratic  function 

n 

Y,\\Pi-RPi\\2,  (10-57) 

i— 1 

where  pt  =  p,  —  p  and  p'  =  p'i  —  p'  are  the  deviations  with  respect  to  the 
centroids. 

It  can  be  proven  that  the  matrix  R  which  minimizes  function  (10.57)  is 
the  matrix  which  maximizes  the  trace  of  RT K,  with 

n 

K  =  '52piPir\ 

i— 1 

see  Problem  10.7.  Therefore,  the  solution  has  the  form  (10.12)  where,  for 
the  purpose  of  this  problem,  U  and  V  are  respectively  the  left  and  right 
orthogonal  matrices  of  the  singular  value  decomposition  of  K.  Once  that 
rotation  matrix  R  is  known,  vector  o  can  be  computed  using  (10.56). 
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10.4.4  3D  Reconstruction  from  Planar  Homography 

Another  interesting  case  of  application  of  3D  reconstruction  occurs  when  the 
feature  points  of  the  observed  object  lie  on  the  same  plane.  This  geometric 
property  represents  an  additional  constraint  between  the  projections  of  each 
point  on  the  image  plane  of  the  two  cameras,  besides  the  epipolar  constraint. 
This  constraint  is  a  planar  homography . 

Let  p2  be  the  position  vector  of  a  point  P  of  the  object,  expressed  with 
respect  to  Frame  2.  Moreover,  let  n2  denote  the  unit  vector  orthogonal  to 
the  plane  containing  the  feature  points  and  d2  >  0  the  distance  of  the  plane 
from  the  origin  of  Frame  2.  By  virtue  of  simple  geometric  considerations,  the 
following  equation  can  be  derived: 

1  2T  2  i 
— n  p =  1 

a2 

which  defines  the  set  of  points  p2  belonging  to  the  plane.  In  view  of  the  above 
equality,  Eq.  (10.43)  can  be  rewritten  in  the  form 

p1  =  Hp 2,  (10.58) 

with 

H  =  R\  +  ——  oj[  2n2T.  (10.59) 

«2 

Replacing  (10.44)  into  (10.58)  yields 

si  =  XHs-2,  (10.60) 

where  A  =  A2/A1  >  0  is  an  arbitrary  constant.  Premultiplication  of  both  sides 
of  (10.60)  by  S'(si)  yields  the  equality 

S(h)Hs2  =  0,  (10.61) 

representing  a  planar  homography  defined  by  matrix  H . 

Using  a  derivation  similar  to  that  presented  in  Sect.  10.3.1,  it  is  possible 
to  compute  numerically  matrix  £iT,  up  to  a  scaling  factor  (j,  starting  from 
the  coordinates  of  n  points  of  the  plane,  with  n  >  4. 

The  value  of  the  scaling  factor  £  can  be  computed  using  a  numerical  al¬ 
gorithm  based  on  expression  (10.59)  of  matrix  H ;  once  H  is  known,  it  is 
possible  to  compute  quantities  R 2,  o\  2/d2  and  n2  in  (10.59)  —  actually,  it 
can  be  shown  that  two  admissible  solutions  exist. 

This  result  is  of  a  certain  relevance  for  visual  servoing  applications.  For 
example,  in  the  case  of  a  camera  in  motion  with  respect  to  the  object,  if 
Frames  1  and  2  represent  the  poses  of  the  camera  in  two  different  time  instants, 
the  computation  of  H  with  decomposition  (10.59)  can  be  used  to  evaluate  the 
orientation  displacement  of  the  camera  frame  and  the  position  displacement 
of  the  origin,  the  latter  defined  up  to  a  scaling  factor  d2.  This  information 
can  be  achieved  without  knowing  the  object  geometry,  as  long  as  the  feature 
points  all  belong  to  the  same  plane. 
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Example  10.5 

Consider  the  SCARA  manipulator  of  Example  10.3  and  the  planar  object  of  Exam¬ 
ple  10.1.  Assume  that  the  feature  vector  of  the  four  points  of  the  object,  measured 
by  the  camera  at  time  t' ,  is 

s'  =  [0  0  0.2  0  0.2  0.2  0  0.2]T, 

while  the  feature  vector,  measured  by  the  camera  at  time  t" ,  is 

s"  =a  |— 0.1667  0.1667  -0.0833  0.0223  0.0610  0.1057  -0.0223  0.2500]T. 

It  is  desired  to  compute  the  quantities  R\,  (l/d2)o\  2  and  n2  of  the  planar  homog- 
raphy  (10.59). 

For  simplicity,  assume  that  the  orientation  of  the  planar  object  is  known,  namely 

n2  =  [  0  0  1]T. 

A  further  simplification  to  the  problem  derives  from  the  fact  that,  in  this  case, 
matrix  R\  corresponds  to  a  rotation  about  axis  2  of  an  angle  /?,  namely  R\  =  Rz(/3). 
Therefore,  in  view  of  (10.59),  planar  homography  H  has  the  symbolic  expression 

C@  —S(3  Ox/d2 

H  =  S/3  Cf3  Oy/d2  , 

0  0  1  +  oz  /  d2  _ 

with  oJ  2  =  [°x  ov  oz]T . 

On  the  other  hand,  starting  from  numerical  values  of  s'  and  s"  and  using  a 
derivation  similar  to  that  used  in  Example  10.1,  the  following  matrix  can  be  ob¬ 
tained: 

'0.3015  -0.5222  0.1373' 

c  H  =  0.5222  0.3015  0.0368  , 

0  0  0.5025. 

which  is  the  numerical  value  of  the  planar  homography,  up  to  a  scaling  factor  (). 

The  symbolic  expression  of  H  reveals  that  the  first  and  second  column  of  this 
matrix  have  unit  norm.  This  property  can  be  used  to  compute  |()|  =  1.6583.  The 
sign  of  £  can  be  evaluated  by  imposing,  for  any  of  the  feature  points  of  the  object, 
the  constraint 

p1Tpx  =  p1T Hp2  =  A i\2si  HH2  >  0. 

Since  scalars  A i  are  positive,  this  inequality  is  equivalent  to 

Si  Hs2  >  0, 

hence,  in  this  case,  it  is  £  >  0.  Therefore 

'  0.5  -0.8660  0.2277' 

H  =  0.8660  0.5  0.0610  . 

0  0  0.8333. 

At  this  point,  the  value  of  angle  (3  can  be  computed  from  the  elements  hu  and 
h2i  of  matrix  H  using  equation 

(3  =  Ataii2(/i2i,  hn)  = 
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Finally,  vector  (l/d2)o}.2  can  be  computed  from  the  elements  of  the  last  row  of 
matrix  H  using  the  equality 


hi3 

'  0.2277  ' 

tl23 

= 

0.0610 

1 

CO 

CO 

-S2 

.-0.0667. 

Notice  that  the  derivation  illustrated  in  Example  10.5,  in  the  case  that  n 2 
is  unknown  and  Ri,  is  a  generic  rotation  matrix,  becomes  much  more  complex. 


10.5  Camera  Calibration 

An  important  problem  for  visual  servoing  applications  is  the  calibration  of 
the  camera,  which  is  the  sensor  providing  the  information  fed  back  to  the 
controller.  Calibration  consists  of  the  estimation  of  the  intrinsic  parameters, 
characterizing  matrix  fl  defined  in  (5.41),  and  of  the  extrinsic  parameters, 
characterizing  the  pose  of  the  camera  frame  with  respect  to  the  base  frame  (for 
eye-to-end  cameras)  or  to  the  end-effector  frame  (for  eye-in-hand  cameras). 
Various  calibration  techniques  exist,  which  are  based  on  algorithms  similar  to 
those  used  for  the  pose  estimation  of  an  object  with  respect  to  a  camera. 

In  particular,  the  solution  method  of  a  PnP  problem  with  n  coplanar 
points  illustrated  in  Sect.  10.3.1  can  be  directly  used  for  the  computation  of 
the  extrinsic  parameters  of  the  camera,  if  the  intrinsic  parameters  are  known. 

In  fact,  in  view  of  (10.14),  extrinsic  parameters  of  an  eye-to-hand  camera 
can  be  computed  as 

Tbc  =  Tb0{Tc0)~\  (10.62) 

where  matrix  Tca  is  the  output  of  the  algorithm  solving  a  PnP  planar  problem, 
provided  that  matrix  Tba,  expressing  the  position  and  the  orientation  of  the 
object  frame  with  respect  to  the  base  frame,  is  known.  Similarly,  in  view 
of  (10.15),  the  extrinsic  parameters  of  an  eye-in-hand  camera  can  be  computed 
as 

Tec  :  Te0(Tc0)-\  (10.63) 

provided  that  matrix  Tea,  expressing  the  pose  of  the  object  frame  with  respect 
to  the  end-effector  frame,  is  known. 

If  the  intrinsic  parameters  are  not  known,  the  derivation  of  Sect.  10.3.1  has 
to  be  suitably  extended  and  can  be  broken  down  in  the  three  phases  described 
below. 

Phase  1 

A  planar  homography  can  be  computed  starting  from  pixel  coordinates 
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in  place  of  normalized  coordinates  s^.  In  detail,  an  equation  formally  identical 
to  (10.7)  can  be  obtained: 

rVti  1  ]T  =  0,  (10.64) 

where  H ’  is  the  (3  x  3)  matrix 

H’  =  flH ,  (10.65) 

fl  being  the  matrix  of  intrinsic  parameters  (5.41)  and  H  the  matrix  defined 
in  (10.8).  Using  an  algorithm  similar  to  that  presented  in  Sect.  10.3.1,  pla¬ 
nar  homography  (Hr  can  be  computed,  up  to  a  scaling  factor  £,  from  the 
coordinates  of  n  points  of  the  plane,  with  n  >  4. 

Phase  2 

Matrix  fl  can  be  computed  from  the  elements  of  matrix  C,H’ .  In  fact,  taking 
into  account  (10.65),  and  the  definition  of  H  in  (10.8),  yields 

CiK  h’2  h,3]  =  fl[r1  r2  o^0 ], 

where  h’i  denotes  the  1-th  column  of  matrix  H’ .  Computing  rq  and  r2  from 
this  equation,  and  imposing  the  orthogonality  and  unit  norm  constraints  on 
these  vectors,  the  following  two  scalar  equations  are  obtained: 

h!^  fl~T  fl~xh!2  =  0 

h[r  fl~T  fl-lh\  =  h'2T  n~T  fl~xb!2 

which,  being  linear,  can  be  rewritten  in  the  form 

A'b=  0.  (10.66) 

In  the  above  equation,  A'  is  a  (2  x  6)  matrix  of  coefficients  depending  on 
h'lt  h'2,  while  b  =  [b\i  br2  52 2  &13  623  633 ]T ,  where  is  the  generic 

element  of  the  symmetric  matrix 

1/ ax  0  -X0/al 

B  =  nTnl  =  *  l/a2y  - Y0/a2y  .  (10.67) 

*  1  +  X2/a2x  +  Y2/al_ 

By  repeating  Phase  1  k  times,  with  the  same  plane  placed  each  time  in  a 
different  pose,  2k  equations  of  the  form  (10.66)  are  obtained.  These  equations, 
in  the  case  k  >  3,  have  a  unique  solution  7 b  defined  up  to  a  scaling  factor  7. 
From  matrix  7 B,  in  view  of  (10.67),  the  following  expressions  for  the  intrinsic 
parameters  can  be  found: 

Xq  =  —b'13/b'n 


442 


10  Visual  Servoing 


Fig.  10.11.  Example  of  calibration  plane 

>0  =  —b'23/b22 


ax  =  yJ'Y/b'n 


where  U-  •  =  j  and  7  can  be  computed  as 

7  =  &13  +  ^23  +  ^33- 


Phase  3 

Once  the  matrix  fl  of  intrinsic  parameters  has  been  evaluated,  it  is  possible 
to  compute  H  from  H' ,  up  to  a  scaling  factor  £,  using  (10.65)  for  one  of 
the  k  poses  of  the  plane.  Hence,  matrix  Tca  can  be  computed  from  H  as 
for  the  case  of  the  solution  of  a  PnP  problem  shown  in  Sect.  10.3.1.  Finally, 
using  equations  (10.62),  (10.63),  the  extrinsic  parameters  of  the  camera  can 
be  evaluated. 

The  method  illustrated  above  is  merely  conceptual,  because  it  does  not 
provide  satisfactory  solutions  in  the  presence  of  measurement  noise  or  lens 
distortion  —  especially  for  the  intrinsic  parameters;  however,  the  accuracy  of 
the  result  can  be  improved  using  models  that  take  into  account  the  distortion 
phenomena  of  the  lenses,  together  with  nonlinear  optimization  techniques. 

From  the  experimental  point  of  view,  the  calibration  method  described 
above  requires  the  use  of  calibration  planes  where  a  certain  number  of  points 
can  be  easily  detected;  also,  the  position  of  these  points  with  respect  to  a 
suitable  reference  frame  must  be  known  with  high  accuracy.  An  example  of 
calibration  plane  with  a  chessboard  pattern  is  shown  in  Fig.  10.11. 

Finally,  notice  that  a  calibration  method  can  also  be  set  up  starting  from 
the  solution  of  a  nonplanar  PnP  problem,  using  the  direct  linear  transforma¬ 
tion  method. 
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Fig.  10.12.  General  block  scheme  of  position-based  visual  servoing 


10.6  The  Visual  Servoing  Problem 

Visual  measurements  allow  a  robot  to  collect  information  on  the  surrounding 
environment.  In  the  case  of  robot  manipulators,  such  information  is  typically 
used  to  compute  the  end-effector  pose  with  respect  to  an  object  observed  by 
the  camera.  The  objective  of  visual  servoing  is  to  ensure  that  the  end-effector, 
on  the  basis  of  visual  measurements  elaborated  in  real  time,  reaches  and  keeps 
a  (constant  or  time-varying)  desired  pose  with  respect  to  the  observed  object. 

It  is  worth  remarking  that  the  direct  measurements  provided  by  the  visual 
system  are  concerned  with  feature  parameters  in  the  image  plane,  while  the 
robotic  task  is  defined  in  the  operational  space,  in  terms  of  the  relative  pose 
of  the  end-effector  with  respect  to  the  object.  This  fact  naturally  leads  to 
considering  two  kinds  of  control  approaches,  illustrated  by  the  block  schemes 
of  Figs.  10.12  and  10.13,  namely  position-based  visual  servoing ,  also  termed 
visual  servoing  in  the  operational  space ,  and  image-based  visual  servoing ,  also 
termed  visual  servoing  in  the  image  space.  In  these  schemes,  the  case  of  eye- 
in-hand  camera  is  considered;  for  eye-to-hand  cameras,  similar  schemes  can 
be  adopted. 

The  position-based  visual  servoing  approach  is  conceptually  similar  to  the 
operational  space  control  illustrated  in  Fig.  8.2.  The  main  difference  is  that 
feedback  is  based  on  the  real-time  estimation  of  the  pose  of  the  observed 
object  with  respect  to  the  camera  using  visual  measurements.  Estimation 
can  be  performed  analytically  or  using  iterative  numerical  algorithms.  Its 
conceptual  advantage  regards  the  possibility  of  acting  directly  on  operational 
space  variables.  Therefore,  the  control  parameters  can  be  selected  on  the  basis 
of  suitable  specifications  imposed  to  the  time  response  of  the  end-effector 
motion  variables,  both  at  steady  state  and  during  the  transient.  The  drawback 
of  this  approach  is  that,  due  to  the  absence  of  direct  control  of  the  image 
features,  the  object  may  exit  from  the  camera  field  of  view  during  the  transient 
or  as  a  consequence  of  planning  errors;  hence,  the  feedback  loop  turns  out  to 
be  open  due  to  lack  of  visual  measurements  and  instability  may  occur. 

In  the  image-space  visual  servoing  approach,  the  control  action  is  com¬ 
puted  on  the  basis  of  the  error  defined  as  the  difference  between  the  value  of 
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Fig.  10.13.  General  block  scheme  of  image-based  visual  servoing 


the  image  feature  parameters  in  the  desired  configuration  —  computed  using 
perspective  transformation  or  directly  measured  with  the  camera  in  the  de¬ 
sired  pose  —  and  the  value  of  the  parameters  measured  with  the  camera  in  the 
current  pose.  The  conceptual  advantage  of  this  solution  regards  the  fact  that 
the  real-time  estimate  of  the  pose  of  the  object  with  respect  to  the  camera 
is  not  required.  Moreover,  since  the  control  acts  directly  in  the  image  feature 
parameters,  it  is  possible  to  keep  the  object  within  the  camera  field  of  view 
during  the  motion.  However,  due  to  the  nonlinearity  of  the  mapping  between 
the  image  feature  parameters  and  the  operational  space  variables,  singular 
configurations  may  occur,  which  cause  instability  or  saturation  of  the  con¬ 
trol  action.  Also,  the  end-effector  trajectories  cannot  be  easily  predicted  in 
advance  and  may  produce  collisions  with  obstacles  or  joint  limits  violation. 

To  compare  the  two  control  strategies  it  is  also  worth  considering  the  oper¬ 
ating  conditions.  Of  particular  importance  is  the  issue  of  camera  calibration. 
It  is  easy  to  understand  that  position-based  visual  servoing  is  more  sensitive 
to  camera  calibration  errors  compared  to  image-based  visual  servoing.  In  fact, 
for  the  first  approach,  the  presence  of  uncertainties  on  calibration  parameters, 
both  intrinsic  and  extrinsic,  produces  errors  on  the  estimate  of  operational 
space  variables  that  may  be  seen  as  an  external  disturbance  acting  on  the 
feedback  path  of  the  control  loop,  where  disturbance  rejection  capability  is 
low.  On  the  other  hand,  in  the  image-based  visual  servoing  approach,  the 
quantities  used  for  the  computation  of  the  control  action  are  directly  defined 
in  the  image  plane  and  measured  in  pixel  units;  moreover,  the  desired  value 
of  the  feature  parameters  is  measured  using  the  camera.  This  implies  that 
the  uncertainty  affecting  calibration  parameters  can  be  seen  as  a  disturbance 
acting  on  the  forward  path  of  the  control  loop,  where  disturbance  rejection 
capability  is  high. 

A  further  aspect  to  analyze  concerns  knowledge  of  the  geometric  model 
of  the  object.  It  is  evident  that,  for  position-based  visual  servoing,  the  object 
geometry  must  be  known  if  only  one  camera  is  used,  because  it  is  necessary 
for  pose  estimation,  while  it  may  be  unknown  when  a  stereo  camera  system  is 
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employed.  On  the  other  hand,  image-based  visual  servoing  does  not  require,  in 
principle,  knowledge  of  the  object  geometry,  even  for  mono-camera  systems. 

On  the  above  premises,  in  the  following,  the  main  position-based  and 
image-based  visual  servoing  schemes  are  illustrated.  For  both  approaches,  the 
problem  of  regulation  to  a  constant  set-point  is  presented  and  the  object  is 
assumed  to  be  fixed  with  respect  to  the  base  frame.  Without  loss  of  gener¬ 
ality,  the  case  of  a  single  calibrated  camera,  mounted  on  the  manipulator’s 
end-effector,  is  considered  (see  Fig.  10.5);  moreover,  the  end-effector  frame  is 
chosen  so  as  to  coincide  with  the  camera  frame. 
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In  position-based  visual  servoing  schemes,  visual  measurements  are  used  to 
estimate  in  real  time  the  homogeneous  transformation  matrix  Tc0,  represent¬ 
ing  the  relative  pose  of  the  object  frame  with  respect  to  the  camera  frame. 
From  matrix  Tc0,  the  (m  x  1)  vector  of  independent  coordinates  xCj0l  defined 
in  (10.31),  can  be  extracted. 

Assumed  that  the  object  is  fixed  with  respect  to  the  base  frame,  the 
position-based  visual  servoing  problem  can  be  formulated  by  imposing  a  de¬ 
sired  value  to  the  relative  pose  of  the  object  frame  with  respect  to  the  camera 
frame.  This  quantity  can  be  specified  in  terms  of  homogeneous  transformation 
matrix  T d,  where  superscript  d  denotes  the  desired  pose  of  the  camera  frame. 
From  this  matrix,  the  (m  x  1)  operational  space  vector  xdo  can  be  extracted. 

Matrices  Tca  and  can  be  used  to  obtain  the  homogeneous  transforma¬ 
tion  matrix 


rrtd 

C 


Td0(Tc0y 


(10.68) 


expressing  the  position  and  orientation  displacement  of  the  camera  frame  in 
the  current  pose  with  respect  to  the  desired  pose.  From  this  matrix,  a  suitable 
error  vector  in  the  operational  space  can  be  computed,  defined  as 


x  =  — 


Jd,c 


4>d 


(10.69) 


where  4>d  c  is  the  vector  of  the  Euler  angles  extracted  from  rotation  matrix 
Rd.  Vector  x  does  not  depend  on  the  object  pose  and  represents  the  error 
between  the  desired  pose  and  the  current  pose  of  the  camera  frame.  It  is 
worth  observing  that  this  vector  does  not  coincide  with  the  difference  between 
Xd>0  and  xC'0,  but  it  can  be  computed  from  the  corresponding  homogeneous 
transformation  matrices,  using  (10.68),  (10.69). 

The  control  has  to  be  designed  so  that  the  operational  space  error  x  tends 
to  zero  asymptotically. 

Notice  that,  for  the  choice  of  the  set  point  xdtQ,  the  knowledge  of  the  object 
pose  is  not  required.  However,  the  control  objective  can  be  satisfied  provided 
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that  the  desired  pose  of  the  camera  frame  with  respect  to  the  base  frame, 
corresponding  to  the  homogeneous  transformation  matrix 

Td  =  Tc{Tdc)~1=  ^  °*  ,  (10.70) 

belongs  to  the  dexterous  workspace  of  the  manipulator.  If  the  object  is  fixed 
with  respect  to  the  base  frame,  this  matrix  is  constant. 

10.7.1  PD  Control  with  Gravity  Compensation 

Position-based  visual  servoing  can  be  implemented  using  PD  control  with 
gravity  compensation,  suitably  modified  with  respect  to  that  used  for  motion 
control. 

Computing  the  time  derivative  of  (10.69),  for  the  position  part,  gives 

•d  _  "d  rd,  _  • 

°d,c  —  °c  —  °d  ~  Rd  °c 

while,  for  the  orientation  part,  it  gives 

4>d,c  =  T~\<l>dtC)U%c  =  T~\<!>d<c)Rlu,c. 

To  compute  the  above  expressions,  equalities  odd  =  0  and  =  0  have  been 
taken  into  account,  observing  that  od  and  Rd  are  constant.  Therefore,  x  has 
the  expression 

x  =  -T21(4>d,c)  *o  R?  v-  (10-71) 

Since  the  end-effector  frame  and  the  camera  frame  coincide,  the  following 
equality  holds: 

x  = -J  Ad(q,x)q,  (10.72) 

where  the  matrix 

JaAq,x)  =  T~a\^c)  °r  J(q )  (10.73) 

has  the  meaning  of  analytical  Jacobian  of  the  manipulator  in  the  operational 
space,  as  for  the  Jacobian  in  (9.14). 

Position-based  visual  servoing  of  PD  type  with  gravity  compensation  has 
the  expression 

u  =  g{q)  +  JTAd{q,x)(K Px  -  KDJAd(q,x)q),  (10.74) 

analogous  to  motion  control  law  (8.110),  but  using  a  different  definition  of 
operational  space  error.  The  asymptotic  stability  of  the  equilibrium  pose  cor¬ 
responding  to  x  =  0,  under  the  assumption  of  symmetric  and  positive  definite 
matrices  Kp,  Kp,  can  be  proven  using  the  Lyapunov  function 

V (q,  x)  =  i qTB(q)q  +  ^xT K Px  >0  Vq,  x  ±  0, 
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Fig.  10.14.  Block  scheme  of  posit  ion- based  visual  servoing  of  PD  type  with  gravity 
compensation 


similarly  to  the  case  of  control  law  (8.110). 

Notice  that,  for  the  computation  of  control  law  (10.74),  the  estimation  of 
xCt0  and  the  measurements  of  q  and  q  are  required.  Moreover,  the  derivative 
term  can  also  be  chosen  as  ~KDq. 

The  block  scheme  of  position-based  visual  servoing  of  PD  type  with  gravity 
compensation  is  shown  in  Fig.  10.14.  Notice  that  the  sum  block  computing 
error  x  and  that  computing  the  output  of  the  controlled  system  have  a  purely 
conceptual  meaning  and  do  not  correspond  to  algebraic  sums. 


10.7.2  Resolved-velocity  Control 

The  information  deriving  from  visual  measurements  is  computed  at  a  fre¬ 
quency  lower  or  equal  to  the  camera  frame  rate.  This  quantity,  especially  for 
CCD  cameras,  is  at  least  of  one  order  of  magnitude  lower  than  the  typical 
frequencies  used  for  motion  control  of  robot  manipulators.  As  a  consequence, 
in  the  digital  implementation  of  control  law  (10.74),  to  preserve  stability  of 
the  closed-loop  system,  the  control  gains  must  be  set  to  values  much  lower 
than  those  typically  used  for  motion  control;  therefore,  the  performance  of  the 
closed-loop  system  in  terms  of  speed  of  convergence  and  disturbance  rejection 
capability  turns  out  to  be  poor. 

This  problem  can  be  avoided  assuming  that  the  manipulator  is  equipped 
with  a  high-gain  motion  controller  in  the  joint  space  or  in  the  operational 
space.  Neglecting  the  effects  on  the  tracking  errors  deriving  from  manipulator 
dynamics  and  disturbances,  the  controlled  manipulator  can  be  considered  as 
an  ideal  positioning  device.  This  implies  that,  in  the  case  of  joint  space  motion 
control,  the  following  equality  holds: 

q(t)  «  qr(t),  (10.75) 

qr(t)  being  the  imposed  reference  trajectory  for  the  joint  variables. 
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Fig.  10.15.  Block  scheme  of  resolved-velocity  position-based  visual  servoing 


Therefore,  visual  servoing  can  be  achieved  by  computing  the  trajectory 
qr(i)  on  the  basis  of  visual  measurements,  so  that  the  operational  space  track¬ 
ing  error  (10.69)  goes  asymptotically  to  zero. 

To  this  end,  Eq.  (10.72)  suggests  the  following  choice  for  the  joint  space 
reference  velocity: 

Q,  =  x)Kx  (10.76) 

which,  replaced  in  (10.72),  by  virtue  of  equality  (10.75),  yields  the  linear 
equation 

x  +  Kx  =  0.  (10.77) 

This  equality,  for  a  positive  definite  matrix  K ,  implies  that  the  operational 
space  error  tends  to  zero  asymptotically  with  a  convergence  of  exponential 
type  and  speed  depending  on  the  eigenvalues  of  matrix  K ;  the  larger  the 
eigenvalues,  the  faster  the  convergence. 

The  above  scheme  is  termed  resolved-velocity  control  in  the  operational 
space,  because  it  is  based  on  the  computation  of  velocity  qr  from  the  oper¬ 
ational  space  error.  Trajectory  qr(t)  is  computed  from  (10.76)  via  a  simple 
integration. 

The  block  scheme  of  resolved-velocity  position-based  visual  servoing  is 
reported  in  Fig.  10.15.  Also  in  this  case,  the  sum  block  computing  the  error 
x  and  that  computing  the  output  of  the  scheme  have  a  purely  conceptual 
meaning  and  do  not  correspond  to  algebraic  sums. 

Notice  that  the  choice  of  K  influences  the  transient  behaviour  of  the  tra¬ 
jectory  of  the  camera  frame,  which  is  the  solution  to  the  differential  equa¬ 
tion  (10.77).  If  K  is  a  diagonal  matrix  with  the  same  gains  for  the  positional 
part,  the  origin  of  the  camera  frame  follows  the  line  segment  connecting  the 
initial  position  to  the  desired  position.  On  the  other  hand,  the  orientation  tra¬ 
jectory  depends  on  the  particular  choice  of  Euler  angles  and,  more  in  general, 
of  the  orientation  error.  The  possible  choices  of  the  orientation  error  are  those 
presented  in  Sect.  3.7.3,  with  the  appropriate  definition  of  Jacobian  (10.73). 
The  possibility  of  knowing  in  advance  the  trajectory  of  the  camera  is  impor¬ 
tant  because,  during  the  motion,  the  object  may  exit  from  the  camera  field  of 
view,  making  visual  measurements  unavailable. 


10.8  Image-based  Visual  Servoing  449 


10.8  Image-based  Visual  Servoing 

If  the  object  is  fixed  with  respect  to  the  base  frame,  image-based  visual  ser¬ 
voing  can  be  formulated  by  stipulating  that  the  vector  of  the  object  feature 
parameters  has  a  desired  constant  value  sd  corresponding  to  the  desired  pose 
of  the  camera.  Therefore,  it  is  implicitly  assumed  that  a  desired  pose  xdt0 
exists  so  that  the  camera  pose  belongs  to  the  dexterous  workspace  of  the 
manipulator  and 

sd  =  s(xdj0).  (10.78) 

Moreover,  xd^a  is  supposed  to  be  unique.  To  this  end,  the  feature  parameters 
can  be  chosen  as  the  coordinates  of  n  points  of  the  object,  with  n  >  4  for 
coplanar  points  (and  no  triplets  of  collinear  points)  or  n  >  6  in  case  of  non- 
coplanar  points.  Notice  that,  if  the  operational  space  dimension  is  in  <  6,  as 
for  the  case  of  SCARA  manipulator,  a  reduced  number  of  points  can  be  used. 

The  interaction  matrix  Ls{s1zc )  depends  on  variables  s  and  zc  with  zc  = 
[zCji . . .  zCjn]T,  zc>i  being  the  third  coordinate  of  the  generic  feature  point  of 
the  object. 

It  is  worth  noticing  that  the  task  is  assigned  directly  in  terms  of  feature 
vector  sd,  while  pose  xdt0  does  not  need  to  be  known.  In  fact,  sd  can  be 
computed  by  measuring  the  feature  parameters  when  the  object  is  in  the 
desired  pose  with  respect  to  the  camera. 

The  control  law  must  be  designed  so  as  to  guarantee  that  the  image  space 
error 

es  =  sd-  s  (10.79) 

tends  asymptotically  to  zero. 

10.8.1  PD  Control  with  Gravity  Compensation 

Image-based  visual  servoing  can  be  implemented  using  a  PD  control  with 
gravity  compensation  defined  on  the  basis  of  the  image  space  error. 

To  this  end,  consider  the  following  positive  definite  quadratic  form  as  Ly- 
papunov  function  candidate: 

V(q,  es)  =  ^qT  B(q)q  +  ^eTsKPses  >  0  Vq,  es  ^  0,  (10.80) 

with  K ps  symmetric  and  positive  definite  ( k  x  k)  matrix. 

Computing  the  time  derivative  of  (10.80)  and  taking  into  account  the 
expression  (8.7)  of  the  joint  space  dynamic  model  of  the  manipulator  and 
property  (7.49)  yields 

V  =  —qrFq  +  qT(u  -  g(q))  +  e[ KPses.  (10.81) 

Since  sd  =  0  and  the  object  is  fixed  with  respect  to  the  base  frame,  the 
following  equality  holds: 


es  =  -s  =  —J l(s,  zc,  <?)<?, 


(10.82) 


450  10  Visual  Servoing 


Fig.  10.16.  Block  scheme  of  image-based  visual  servoing  of  PD  type  with  gravity 
compensation 


where 


JL(s,zc,q)  =  Ls(s,  zc) 


Rc  O 
O  Rr 


J(q), 


(10.83) 


the  camera  frame  and  the  end-effector  frame  being  coincident. 
Therefore,  with  the  choice 


u  =  g(q)  +  J  l(s,  zc,  q)  ( KPses  -  K  DsJ  L(s,  zc,  q)q) , 


(10.84) 


where  Kp>s  is  a  symmetric  and  positive  definite  ( k  x  k)  matrix,  Eq.  (10.81) 
becomes 

V  =  ~qTFq-qTJTLKDsJLq.  (10.85) 

Control  law  (10.84)  includes  a  nonlinear  compensation  action  of  gravita¬ 
tional  forces  in  the  joint  space  and  a  linear  PD  action  in  the  image  space.  The 
last  term,  in  view  of  (10.82),  corresponds  to  a  derivative  action  in  the  image 
space  and  has  been  added  to  increase  damping.  The  resulting  block  scheme  is 
reported  in  Fig.  10.16. 

The  direct  measurement  of  s  would  permit  the  computation  of  the  deriva¬ 
tive  term  as  —  K £>ss;  this  measurement,  however,  is  not  available.  As  an  alter¬ 
native,  the  derivative  term  can  simply  be  set  as  —Kpq,  with  Kp>  symmetric 
and  positive  definite  (n  x  n)  matrix. 

Equation  (10.85)  reveals  that,  for  all  trajectories  of  the  system,  the  Lya¬ 
punov  function  decreases  until  g  /  0.  Therefore  the  system  reaches  an  equi¬ 
librium  state,  characterized  by 

j£(s,  zc,  q)KPses  =  0.  (10.86) 


Equations  (10.86),  (10.83)  show  that,  if  the  interaction  matrix  and  the  geo¬ 
metric  Jacobian  of  the  manipulator  are  full  rank,  then  es  =  0,  which  is  the 
sought  result. 
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Fig.  10.17.  Block  scheme  of  resolved- velocity  image-based  visual  servoing 


Notice  that  control  law  (10.84)  requires  not  only  the  measurement  of  s  but 
also  the  computation  of  vector  zc  which,  in  the  image-based  visual  servoing 
philosophy,  should  be  avoided.  In  some  applications  zc  is  known  with  good 
approximation,  as  in  the  case  that  the  relative  motion  of  the  camera  with 
respect  to  the  object  belongs  to  a  plane.  Alternatively,  estimated  or  constant 
values  can  be  used  for  zc,  as  the  value  in  the  initial  configuration  or  that  in 
the  desired  configuration.  This  is  equivalent  to  using  an  estimate  Ls  of  the 
interaction  matrix.  In  such  cases,  however,  the  stability  proof  becomes  much 
more  complex. 

10.8.2  Resolved-velocity  Control 

The  concept  of  resolved-velocity  control  can  easily  be  extended  to  the  image 
space.  In  such  a  case,  Eq.  (10.82)  suggests  the  following  choice  of  the  reference 
velocity  in  joint  space 

Qr  =  Jl1^’  zc,  <7 r)Kses,  (10.87) 

under  the  assumption  of  invertible  matrix  J  jj  .  This  control  law,  replaced 
in  (10.82),  yields  the  linear  equation 

es  +  Kses  =  0.  (10.88) 

Therefore,  if  Ks  is  a  positive  definite  matrix,  Eq.  (10.88)  is  asymptotically  sta¬ 
ble  and  error  es  tends  asymptotically  to  zero  with  convergence  of  exponential 
type  and  speed  depending  on  the  eigenvalues  of  matrix  K s.  The  convergence 
to  zero  of  the  image  space  error  es  ensures  the  asymptotic  convergence  of  xco 
to  the  desired  pose  Xd,0- 

The  block  scheme  of  resolved- velocity  image-based  visual  servoing  is  shown 
in  Fig.  10.17. 

Notice  that  this  control  scheme  requires  the  computation  of  the  inverse  of 
matrix  therefore  it  is  affected  by  problems  related  to  the  singularities  of 
this  matrix  which,  in  view  of  (10.83),  are  both  those  of  the  geometric  Jacobian 
and  those  of  the  interaction  matrix.  The  most  critical  singularities  are  those 
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of  the  interaction  matrix,  since  they  depend  on  the  choice  of  the  image  feature 
parameters. 

Therefore,  it  is  convenient  to  compute  control  law  (10.87)  in  two  steps. 
The  first  step  is  the  computation  of  vector 

vcr  =  L-s\s,zc)Kses.  (10.89) 


The  second  step  is  the  computation  of  the  joint  space  reference  velocity  using 
the  relationship 


Qr 


J~\q ) 


Rc 

O 


(10.90) 


Far  from  the  kinematic  singularities  of  the  manipulator,  the  problem  of 
the  singularities  of  the  interaction  matrix  can  be  overcome  by  using  a  number 
k  of  feature  parameters  greater  than  the  minimum  required  in,  similarly  to 
the  case  considered  in  Sect.  10.3.3.  The  control  law  can  be  modified  by  using 
the  left  pseudo-inverse  of  interaction  matrix  Ls  in  place  of  the  inverse,  namely 


=  {LtsLs) 


(10.91) 


in  place  of  (10.89).  Stability  of  the  closed-loop  system  with  control  law  (10.90), 
(10.91)  can  be  shown  using  the  direct  Lyapunov  method  based  on  the  positive 
definite  function 


V(es)  =  K ses  >  0  Wes  ^  0. 

Computing  the  time  derivative  of  this  function  and  taking  into  account  (10.82), 
(10.83),  (10.90),  (10.91),  yields 

V=  -ejKsLs(LTLs)-1LjKses 

which  is  negative  semi-definite  because  Af(Lj)  ^  0,  L J  being  a  matrix  with 
more  columns  than  rows.  Therefore,  the  closed-loop  system  is  stable  but  not 
asymptotically  stable.  This  implies  that  the  error  is  bounded,  but  in  some 
cases  the  system  may  reach  an  equilibrium  with  es  ^  0  and  Kses  £  Af(L^). 

Another  problem  connected  with  the  implementation  of  control  law  (10.89) 
or  (10.91)  and  (10.90)  depends  on  the  fact  that  the  computation  of  interaction 
matrix  Ls  requires  knowledge  of  zc.  Similar  to  Sect.  10.8.1,  this  problem  can 

be  solved  by  using  an  estimate  of  matrix  Ls  (or  of  its  pseudo-inverse).  In  this 
case,  the  Lypapunov  method  can  be  used  to  prove  that  the  control  scheme 

^-i 

remains  stable  provided  that  matrix  LSLS  is  positive  definite.  Notice  that  zc 
is  the  only  information  depending  on  object  geometry.  Therefore,  it  can  also 
be  seen  that  image-based  visual  servoing,  in  the  case  that  only  one  camera  is 
used,  does  not  require  exact  knowledge  of  object  geometry. 

The  choice  of  the  elements  of  matrix  K s  influences  the  trajectories  of  the 
feature  parameters,  which  are  solution  to  differential  equation  (10.88).  In  the 
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case  of  feature  points,  if  a  diagonal  matrix  Ks  with  equal  elements  is  set,  the 
projections  of  these  points  on  the  image  plane  will  follow  line  segments.  The 
corresponding  camera  motion,  however,  cannot  be  easily  predicted,  because  of 
the  nonlinearity  of  the  mapping  between  image  plane  variables  and  operational 
space  variables. 


10.9  Comparison  Among  Various  Control  Schemes 

In  order  to  make  a  comparison  between  the  various  control  schemes  presented, 
consider  the  SCARA  manipulator  of  Example  10.3  and  the  planar  object  of 
Example  10.1.  The  base  frame  of  the  SCARA  manipulator  of  Fig.  2.36  is  set 
with  the  origin  at  the  intersection  of  the  axis  of  joint  1  with  the  horizontal 
plane  containing  the  origin  of  the  end-effector  frame  when  =  0,  d3  being 
the  prismatic  joint  variable;  the  axis  z  of  the  base  frame  points  downward. 
The  operational  space,  of  dimension  m  =  4,  is  characterized  by  vector  xc^0 
in  (10.33). 

With  reference  to  the  dynamic  model  of  Problem  7.3,  the  same  data  of 
Example  7.2  are  considered;  in  addition,  =  2kg  and  Ig4  =  1  kg-m2,  while 
the  contribution  of  the  motors  of  the  last  two  links  are  neglected. 

For  position-based  visual  servoing  schemes,  the  real-time  estimation  of 
vector  £cCi0  from  a  suitable  feature  vector  is  required.  To  this  end,  the  algo¬ 
rithm  of  Sect.  10.3.3  can  be  used,  based  on  the  inverse  of  the  image  Jacobian. 
This  is  a  classical  visual  tracking  application  that  can  be  accomplished  using 
only  two  feature  points,  because  the  corresponding  Jacobian  J as  is  a  (4  x  4) 
matrix.  The  selected  points  are  Pi  and  P2  of  the  object  of  Fig.  10.6. 

The  same  points  can  also  be  used  for  image-based  visual  servoing,  because 
the  corresponding  Jacobian  Jl  is  a  (4  x  4)  matrix. 

It  is  assumed  that  at  time  t  =  0  the  pose  of  the  camera  frame,  with  respect 
to  the  base  frame,  is  defined  by  the  operational  space  vector 

£CC(0)  =  [1  1  0.5  tt/4]T 

and  the  pose  of  the  object  frame,  with  respect  to  the  camera  frame,  is  defined 
by  the  operational  space  vector 

*c>o(0)  =  [0  0  0.5  Of. 

The  desired  pose  of  the  object  frame  with  respect  to  the  camera  frame  is 
defined  by  vector 

&d,o  =  [—0.1  0.1  0.6  -tt/3]t. 

This  quantity  is  assumed  as  the  initial  value  of  the  pose  estimation  algorithm 
used  by  position-based  visual  servoing  schemes. 
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For  image-based  visual  servoing  schemes,  the  desired  value  of  the  feature 
parameters  of  points  P\  and  P2  of  the  object,  in  the  desired  pose  xd^Q,  is 

sd  =  [-0.1667  0.1667  -0.0833  0.0223  }T . 

For  all  the  schemes,  a  discrete-time  implementation  of  the  controller  with 
sampling  time  of  0.04  s  has  been  adopted,  corresponding  to  a  25  Hz  frequency. 
This  value  coincides  with  the  minimum  frame  rate  of  analog  cameras  and 
allows  the  use  of  visual  measurements  also  in  the  worst  case. 

In  the  numerical  simulations,  the  following  control  schemes  have  been  uti¬ 
lized: 

A.  Position-based  visual  servoing  of  PD  type  with  gravity  compensation  with 
the  following  data: 


KP  =  diag{500, 500, 10, 10} 

Kd  =  diag{500, 500, 10, 10}. 

B.  Resolved- velocity  position-based  visual  servoing  with  the  following  data: 

K  =  diag{l,  1, 1, 2}, 

corresponding  to  a  time  constant  of  1  s  for  the  three  position  variables  and 
of  0.5  s  for  the  orientation  variable. 

C.  Image-based  visual  servoing  of  PD  type  with  gravity  compensation  with 
the  following  data: 


Kps  =  300/4  K  Ds  =  330/4. 

D.  Resolved- velocity  image-based  visual  servoing  with  the  following  data: 

Ks  =  /4, 

corresponding  to  a  time  constant  of  1  s  for  the  feature  parameters. 

For  the  simulation  of  resolved- velocity  control  schemes,  the  dynamics  of 
the  velocity  controlled  manipulator  has  been  neglected.  Therefore,  a  pure 
kinematic  model  has  been  considered,  based  on  the  analytical  Jacobian  of  the 
manipulator. 

For  position-based  control  schemes,  the  pose  estimation  algorithm  based 
on  the  inverse  of  the  image  Jacobian  has  been  adopted,  with  integration  step 
At  =  1  ms  and  gain  Ks  =  I6O/4.  As  shown  in  Example  10.4,  this  implies 
that  the  algorithm  converges  in  a  time  of  about  0.03  s,  which  is  lower  than  the 
sampling  time  of  the  control,  as  required  for  a  correct  operation  of  position- 
based  control. 

For  image-based  control  schemes,  matrix  Ls{s1  zc )  has  been  approximated 
with  matrix  Ls  =  Ls(s ,  zd),  where  zd  is  the  third  component  of  vector  xdo. 
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The  parameters  of  the  various  controllers  have  been  chosen  in  such  a  way 
as  to  show  the  particular  features  of  the  different  control  laws  and,  at  the  same 
time,  to  allow  a  significant  comparison  of  the  performance  of  each  scheme  in 
response  to  congruent  control  actions.  In  particular,  it  can  be  observed  what 
follows: 

•  The  gains  in  schemes  A  and  C  have  been  tuned  in  simulation  so  as  to 
obtain  transient  behaviors  similar  to  those  of  schemes  B  and  D. 

•  In  control  scheme  B  the  gains  of  the  position  variables  have  been  inten¬ 
tionally  chosen  equal  to  one  another  but  different  from  the  gain  of  the 
orientation  variable  to  show  that  a  desired  dynamics  can  be  imposed  to 
each  operational  space  variable. 

•  In  control  scheme  D  the  gains  have  all  been  chosen  equal  to  one  another, 
since  imposing  different  dynamics  to  different  coordinates  of  the  projec¬ 
tions  of  the  feature  points  on  the  image  plane  is  not  significant. 

The  results  obtained  with  the  various  control  schemes  are  illustrated  in 
Figs.  10.18-10.25  in  terms  of: 

•  The  time  history  of  position  and  orientation  of  the  camera  frame  with 
respect  to  the  base  frame  and  corresponding  desired  values  (represented 
with  dashed  lines). 

•  The  time  history  of  feature  parameters  and  corresponding  desired  values 
(represented  with  dashed  lines). 

•  The  path  of  feature  points  projections  on  the  camera  image  plane,  from 
initial  positions  (marked  with  crosses)  to  final  positions  (marked  with  cir¬ 
cles)  . 

Regarding  performance  of  the  various  control  schemes,  the  following  con¬ 
siderations  can  be  drawn  from  the  obtained  results. 

In  principle,  if  position-based  visual  servoing  is  adopted,  a  desired  tran¬ 
sient  behaviour  can  be  assigned  to  the  operational  space  variables.  This  is  only 
partially  true  for  control  scheme  A,  because  the  dynamics  of  the  closed-loop 
system,  for  PD  control  with  gravity  compensation,  is  nonlinear  and  coupled. 
Therefore,  the  transient  behaviour  shown  in  Fig.  10.18  may  be  different  if 
the  manipulator  starts  from  a  different  initial  pose  or  has  to  reach  a  different 
desired  pose.  Vice  versa,  for  control  scheme  B,  the  time  history  of  operational 
space  variables  reported  in  Fig.  10.20  shows  transient  behaviours  of  exponen¬ 
tial  type,  whose  characteristics  depend  only  on  the  choice  of  matrix  K . 

For  both  schemes  A  and  B,  the  trajectories  of  the  projections  of  the  feature 
points  and  the  corresponding  paths  in  the  image  plane  (Figs.  10.19  and  10.21 
respectively)  have  evolutions  that  cannot  be  predicted  in  advance.  This  implies 
that,  although  the  feature  points  projections  are  inside  the  image  plane  both  in 
the  initial  and  in  the  desired  configuration,  they  may  exit  from  the  image  plane 
during  the  transient,  thus  causing  problems  of  convergence  to  the  controlled 
system. 
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Fig.  10.18.  Time  history  of  camera  frame  position  and  orientation  with  control  A 


Fig.  10.19.  Time  history  of  feature  parameters  and  corresponding  path  of  feature 
points  projections  on  image  plane  with  control  A 


If  image-based  control  is  adopted,  a  desired  transient  behaviour  can  be 
assigned  to  the  time  histories  of  feature  parameters  and  not  to  the  operational 
space  variables,  in  a  dual  fashion  with  respect  to  position-based  control.  This 
is  confirmed  by  the  results  shown  in  Figs.  10.22-10.25,  relative  to  control 
schemes  C  and  D,  respectively.  In  detail,  especially  in  the  case  of  control  C, 
the  time  histories  of  the  operational  space  variables  are  quite  different  from 
those  reported  in  Figs.  10.18  and  10.20,  despite  the  same  initial  and  final 
configuration  and  a  similar  transient  duration.  This  implies  that  the  camera 
path,  being  unpredictable,  may  lead  to  joint  limits  violation  or  to  collision  of 
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Fig.  10.20.  Time  history  of  camera  frame  position  and  orientation  with  control  B 
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Fig.  10.21.  Time  history  of  feature  parameters  and  corresponding  path  of  feature 
points  projections  on  image  plane  with  control  B 


the  manipulator  with  an  obstacle.  In  the  specific  case  of  control  scheme  C,  a 
300%  overshoot  is  present  on  the  z  component  of  the  camera  trajectory  (see 
Fig.  10.22),  which  corresponds  to  a  camera  retreat  movement  with  respect  to 
the  object  of  amplitude  much  higher  than  the  distance  reached  at  the  end  of 
the  transient.  The  overshoot  on  the  z  component  is  present  also  for  control 
scheme  D,  but  is  ‘only’  of  50%  (see  Fig.  10.24). 

Notice  that,  for  control  scheme  C,  the  presence  of  large  displacements  on 
some  operational  space  variables  does  not  correspond  to  significant  deviations 
of  the  feature  parameters  with  respect  to  their  final  values  during  the  transient 
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Fig.  10.22.  Time  history  of  camera  frame  position  and  orientation  with  control  C 


Fig.  10.23.  Time  history  of  feature  parameters  and  corresponding  path  of  feature 
points  projections  on  image  plane  with  control  C 


(see  Fig.  10.23).  Indeed,  the  paths  of  the  feature  points  projections  do  not 
deviate  much  from  the  line  segments  connecting  these  points. 

Figure  10.25,  relative  to  control  scheme  D,  reveals  that  the  trajectories 
of  the  feature  parameters  are  of  exponential  type.  In  this  case,  the  transient 
behaviour  depends  only  on  matrix  K s ;  choosing  a  diagonal  matrix  with  equal 
elements  implies  that  the  paths  of  the  feature  points  projections  are  linear. 
In  the  case  at  hand,  in  view  of  the  approximation  Ls(s,zc)  ss  Ls{s,Zd),  the 
paths  of  the  feature  points  projections  shown  in  Fig.  10.25  are  not  perfectly 
linear. 
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Fig.  10.24.  Time  history  of  camera  frame  position  and  orientation  with  control  D 


Fig.  10.25.  Time  history  of  feature  parameters  and  corresponding  path  of  feature 
points  projections  on  image  plane  with  control  D 


To  conclude,  Fig.  10.26  shows  the  paths  of  the  origin  of  the  camera  frame 
obtained  using  the  four  control  schemes.  It  can  be  observed  that,  with  control 
scheme  B,  a  perfectly  linear  path  is  obtained,  thanks  to  the  choice  of  a  diagonal 
gain  matrix  Ks  with  equal  weights  for  the  positional  part.  Using  control 
scheme  A,  the  path  is  almost  linear,  because,  unlike  case  B,  this  type  of 
control  does  not  guarantee  a  decoupled  dynamics  for  each  operational  space 
variable.  Vice  versa,  using  control  schemes  C  and  D,  the  path  of  the  origin  of 
the  camera  frame  is  far  from  being  linear.  In  both  cases,  the  phenomenon  of 
camera  retreat  with  respect  to  the  object  can  be  observed.  To  this  end,  notice 
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scheme  A 


[m]  [m] 

scheme  C 


[m]  [m] 


Fig.  10.26.  Paths  of  camera  frame 
schemes 


scheme  B 


[m]  [m] 

scheme  D 


in  Cartesian  space  with  the  four  control 


the  axis  2  of  the  base  frame  of  the  SCARA  manipulator  and  the  axis  zc  of 
the  camera  frame  are  aligned  to  the  vertical  direction  and  point  downward; 
therefore,  with  respect  to  the  reference  frames  of  Fig.  10.26,  the  object  is  on 
the  top  and  the  camera  points  upward. 

The  phenomenon  of  camera  retreat  appears  whenever  the  camera  is  re¬ 
quired  to  perform  large  rotations  about  the  optical  axis;  this  phenomenon 
can  be  intuitively  explained  through  a  simple  example.  Assume  that  a  pure 
rotation  of  the  camera  about  the  axis  zc  is  required  and  control  scheme  D 
is  adopted.  Therefore,  visual  servoing  imposes  that  the  feature  points  projec¬ 
tions  on  the  image  plane  follow  rectilinear  paths  from  the  initial  to  the  desired 
positions,  whereas  simple  camera  rotation  would  have  required  circular  paths. 
The  constraint  on  the  path  in  the  image  plane  implies  that,  during  rotation, 
the  origin  of  the  camera  frame  must  move,  with  respect  to  the  object,  first 
backward  and  then  forward  to  reach  again,  asymptotically,  the  initial  position. 
It  can  be  shown  that,  if  the  desired  rotation  tends  to  n,  then  the  distance  of 
the  camera  from  the  object  tends  to  oo  and  the  system  becomes  unstable. 


10.10  Hybrid  Visual  Servoing 

An  approach  which  combines  the  benefits  of  position-based  and  image-based 
visual  servoing  is  hybrid  visual  servoing.  The  name  stems  from  the  fact  that 


10.10  Hybrid  Visual  Servoing  461 


the  control  error  is  defined  in  the  operational  space  for  some  components  and 
in  the  image  space  for  the  others.  This  implies  that  a  desired  motion  can  be 
specified,  at  least  partially,  in  the  operational  space  so  that  the  camera  trajec¬ 
tory  during  visual  servoing  can  be  predicted  in  advance  for  some  components. 
On  the  other  hand,  the  presence  of  error  components  in  the  image  space  helps 
keep  the  image  features  in  the  camera  field  of  view,  which  is  a  difficult  task 
in  position-based  approaches. 

Hybrid  visual  servoing  requires  the  estimation  of  some  operational  space 
variables.  Assume  that  the  object  has  a  planar  surface  where  at  least  four 
feature  points,  and  no  triplets  of  collinear  points,  can  be  selected.  Using  the 
coordinates  of  these  points  in  the  camera  image  plane,  both  in  the  current  and 
in  the  desired  pose  of  the  camera  frame,  it  is  possible  to  compute  the  planar 
homography  H  as  described  in  Sect.  10.4.4.  Notice  that,  for  this  computation, 
knowledge  of  the  current  and  the  desired  camera  pose  is  not  required,  provided 
that  the  feature  vectors  s  and  sd  are  known. 

In  view  of  (10.59),  assuming  that  Frame  1  coincides  with  the  camera  frame 
in  the  current  pose  and  Frame  2  coincides  with  the  camera  frame  in  the  desired 
pose,  the  following  equality  holds: 


H  =  Rcd  +  TocCtdri 


dT 


where  Rcd  is  the  rotation  matrix  between  the  desired  orientation  and  the 
current  orientation  of  the  camera  frame,  occ  d  is  the  position  vector  of  the 
origin  of  the  camera  frame  in  the  desired  pose  with  respect  to  the  current 
pose,  nd  is  the  unit  vector  normal  to  the  plane  containing  the  feature  points, 
and  dd  is  the  distance  between  this  plane  and  the  origin  of  the  camera  frame 
in  the  desired  pose.  The  quantities  Rd,  nd,  (1  /dd)occ  d,  in  the  current  camera 
pose,  can  be  computed  at  each  sampling  time  from  matrix  if. 

Adopting  a  resolved- velocity  approach,  the  control  objective  consists  of 
computing  the  reference  absolute  velocity  of  the  camera  frame 


v 


C 

r 


from  a  suitably  defined  error  vector. 

To  this  end,  the  orientation  error  between  the  desired  and  the  current 
camera  pose  can  be  computed  from  matrix  Rd,  as  for  position-based  visual 
servoing.  If  4>c  d  denotes  the  vector  of  the  Euler  angles  extracted  from  Rd,  the 
control  vector  can  be  chosen  as 


«r  =  -T(<t>c4)K0<fic4,  (10.92) 

where  Ka  is  a  (3  x  3)  matrix.  With  this  choice,  the  equation  of  the  orientation 
error  has  the  form 


0c, d  +  Ko4>c,d  ~  0. 


(10.93) 
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Equation  (10.93),  if  Ka  is  a  symmetric  and  positive  definite  matrix,  implies 
that  the  orientation  error  tends  to  zero  asymptotically  with  convergence  of 
exponential  type  and  speed  depending  on  the  eigenvalues  of  matrix  Ka. 

The  control  vector  should  be  selected  so  that  the  positional  part  of  the 
error  between  the  desired  and  the  current  camera  pose  converges  to  zero.  The 
position  error  could  be  defined  as  the  difference  of  the  coordinates  of  a  point 
of  the  object  in  the  desired  camera  frame  rd  =  [xd  yd  Zd]T  and  those  in 
the  current  camera  frame  rcc  =  [xc  yc  zc  ]T,  namely  rd  —  rcc.  These  coordi¬ 
nates,  however,  cannot  be  directly  measured,  unlike  the  corresponding  coor¬ 
dinates  in  the  image  plane,  defining  the  feature  vectors  sPjd  =  [Xd  Yd]T  = 
[xd/zd  Vd/zd\T  and  sp  =  [X  Y }T  =  [ xc/zc  yc/zc}T . 

The  information  deriving  from  the  computation  of  homography  H  can  be 
used  to  rewrite  the  ratio 

Pz  =  Zc/Zd 

in  terms  of  known  or  measurable  quantities  in  the  form 


with 


Pz  = 


—  —  1  +  n 
dd 


dc  ndTsP}d 
dd  ncTsp 

(10.94) 

=det(H), 

dd 

(10.95) 

and  nc  =  Rcdnd ,  where  vectors  sp  and  'sP}d  denote  the  representations  in 
homogeneous  coordinates  of  sp  and  sPi,j,  respectively  (see  Problem  10.12). 

The  position  error,  expressed  in  terms  of  known  or  measurable  quantities, 
can  be  defined  as 

f Xd-Xl 


ep{rdXcc) 


Yd-Y 
In  pz 


Notice  that,  in  view  of  (5.44),  convergence  to  zero  of  ep  implies  convergence 
to  zero  of  rcd  —  rcc  and  vice  versa. 

Computing  the  time  derivative  of  ep  yields 


e 


p  ~ 


9ep(rcc) 

dn  c’ 


rd  being  constant.  By  taking  into  account  (10.26)  and  the  decomposition 


LjC 


with  ucc  =  bc,  the  above  expression  can  be  rewritten  in  the  form 


ep  =  -JpK  -  J0ucc,x 


(10.96) 
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Fig.  10.27.  Time  history  of  position  and  orientation  of  camera  frame  with  hybrid 
visual  servoing 


with  (see  Problem  10.13) 
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Equation  (10.96)  suggests  the  following  choice  of  control  vector  i/£ 


V r  Jp  (K-pGp  J 0LO  r), 


(10.97) 


Jp  being  a  nonsingular  matrix. 

Notice  that,  for  the  computation  of  Jp  ,  knowledge  of  the  constant  quan¬ 
tity  Zd  is  required. 

If  Zd  is  known,  control  law  (10.97),  in  view  of  assumptions  occ  «  vcr  and 
ujcc  «  yields  the  following  error  equation: 


bp  T  KpfZp  —  0, 
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Fig.  10.28.  Time  history  of  feature  parameters  and  corresponding  path  of  feature 
points  projections  on  image  plane  with  hybrid  visual  servoing 


which  implies  the  exponential  convergence  of  ep  to  zero,  provided  that  Kp  is 
a  positive  definite  matrix. 

If  Zd  is  not  known,  an  estimate  z,i  can  be  adopted.  Therefore,  in  control 
law  (10.97),  matrix  1  can  be  replaced  by  an  estimate  J p  .  In  view  of  the 
equality 


the  following  error  equation  is  obtained: 

ep  +  —Kpep  =  (l  -  — \  J0^r- 
Zd  \  zdJ 

This  equation  shows  that  the  use  of  an  estimate  z,i  in  place  of  the  true  value 
Zd  implies  a  simple  gain  scaling  in  the  error  equation,  and  asymptotic  stability 
is  preserved.  Moreover,  due  to  the  presence  in  the  right-hand  side  of  the  above 
equation  of  a  term  depending  on  <jjcr,  the  time  history  of  ep  is  influenced  by 
the  orientation  error,  which  evolves  according  to  (10.93). 


Example  10.6 

For  the  SCARA  manipulator  and  the  task  of  Sect.  10.9,  consider  the  hybrid  visual 
servoing  law  with  gains 

Kp  =  1 3  k0  =  2, 

and  compute  the  positional  part  of  the  error  with  respect  to  point  Pi.  The  planar 
homography  and  the  corresponding  parameters  are  estimated  as  in  Example  10.5, 
using  four  points.  The  results  are  reported  in  Figs.  10.27  and  10.28,  in  terms  of  the 
same  variables  shown  in  Sect.  10.9.  Notice  that  the  time  histories  of  the  variables  in 
the  operational  space  of  Fig.  10.27  are  quite  similar  to  that  obtained  with  resolved- 
velocity  position-based  visual  servoing  (Fig.  10.20).  On  the  other  hand,  the  time 
histories  of  the  feature  parameters  of  Fig.  10.28  are  substantially  similar  to  those 
obtained  with  resolved-velocity  image-based  visual  servoing  (Fig.  10.25)  except  for 
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Fig.  10.29.  Path  of  camera  frame  origin  in  Cartesian  space  with  hybrid  visual 
servoing 


the  fact  that  the  path  in  the  image  plane  of  the  projection  of  point  Pi  is  perfectly 
linear,  as  imposed  by  the  control.  The  corresponding  path  of  the  camera  frame 
origin,  reported  in  Fig.  10.29,  shows  a  substantial  improvement  with  respect  to 
those  obtained  with  the  image-based  visual  servoing  schemes  of  Fig.  10.26. 


The  method  illustrated  above  is  only  one  of  the  possible  visual  servoing 
approaches  based  on  the  computation  of  the  planar  homography  and  on  its 
decomposition.  It  is  worth  pointing  out  that  knowledge  of  (1  /dd)occ  d  and  Rcd 
allows  the  computation  of  the  operational  space  error  (10.69),  up  to  a  scaling 
factor  for  the  positional  part;  therefore,  it  is  also  possible  to  use  the  position- 
based  visual  servoing  schemes  presented  in  Sect.  10.7.  On  the  other  hand,  in 
hybrid  visual  servoing  approaches,  different  choices  are  possible  for  the  error 
components  depending  on  feature  parameters  as  well  as  for  those  depending 
on  the  operational  space  variables. 
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Problems 

10.1.  Derive  Eq.  (10.2). 

10.2.  Show  that  a  non-null  solution  to  (10.10)  is  the  right  eigenvector  corre¬ 
sponding  to  the  null  singular  value  of  matrix  A. 

10.3.  Show  that  (10.12)  is  the  matrix  which  minimizes  Frobenius  norm  (10.11) 
with  the  constraint  that  Rca  is  a  rotation  matrix,  in  the  case  cr  >  0.  [Hint:  con¬ 
sider  that  Tr {RCJ  U  SVT)  =  Tr (VT RcaTU £);  moreover,  the  absolute  values 
of  the  diagonal  elements  of  matrix  VT RC^U  are  lower  or  equal  to  1.] 

10.4.  Consider  the  SCARA  manipulator  of  Example  10.3.  Perform  a  com¬ 
puter  implementation  of  the  pose  estimation  algorithm  based  on  the  inverse 
of  the  image  Jacobian  considering  the  points  P\  and  P2  of  the  object  of  Exam¬ 
ple  10.1.  Compute  the  homogeneous  transformation  matrix  corresponding  to 
the  feature  vector  s  =  [—0.1667  0.1667  —0.0833  0.0223 ]T.  Assume  that, 
in  the  initial  pose,  the  axes  of  the  camera  frame  are  parallel  to  those  of  the 
object  frame  and  the  origin  is  at  a  distance  of  0.5  m  along  the  vertical  axis. 

10.5.  Solve  the  previous  problem  using  the  feature  parameters  of  the  line 
segment  P1P2. 

10.6.  Show  that  the  value  of  o  which  minimizes  function  (10.55)  has  expres¬ 

sion  (10.56).  [Hint:  Use  the  equalities  p,  =  p,  +p  and  p'  =  p'  +p'  in  (10.55), 
and  the  properties  ||a+b||2  =  ||a||2-|-2aT6-|-||6||2  and  Yn=iPi  =  =  °] 

10.7.  Show  that  the  matrix  R  which  minimizes  function  (10.57)  is  the  matrix 
which  maximizes  the  trace  of  RT  K . 

10.8.  For  the  SCARA  manipulator  of  Example  10.3,  design  a  position-based 
visual  servoing  scheme  of  PD  type  with  gravity  compensation  using  the  mea¬ 
surement  of  the  feature  parameters  of  line  segment  P1P2  of  Example  10.1. 
Perform  a  computer  simulation  in  the  same  operating  conditions  of  Sect.  10.9 
and  compare  the  results. 

10.9.  For  the  SCAR  A  manipulator  of  Example  10.3,  design  a  resolved- velocity 
position-based  visual  servoing  scheme  using  the  measurement  of  the  feature 
parameters  of  line  segment  P1P2  of  Example  10.1.  Perform  a  computer  simu¬ 
lation  in  the  same  operating  conditions  of  Sect.  10.9  and  compare  the  results. 
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10.10.  For  the  SCARA  manipulator  of  Example  10.3,  design  an  image-based 
visual  servoing  scheme  of  PD  type  with  gravity  compensation  using  the  mea¬ 
surement  of  the  feature  parameters  of  the  line  segment  P1P2  of  Example  10.1. 
Perform  a  computer  simulation  in  the  same  operating  conditions  of  Sect.  10.9 
and  compare  the  results. 

10.11.  For  the  SCARA  manipulator  of  Example  10.3,  design  a  resolved- 
velocity  image-based  visual  servoing  scheme  using  the  measurement  of  the 
feature  parameters  of  line  segment  P1P2  of  Example  10.1.  Perform  a  com¬ 
puter  simulation  in  the  same  operating  conditions  of  Sect.  10.9  and  compare 
the  results. 

10.12.  Derive  the  expressions  (10.94),  (10.95). 

10.13.  Derive  the  expressions  of  Jp  and  Ja  in  (10.96). 
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Mobile  Robots 


The  previous  chapters  deal  mainly  with  articulated  manipulators  that  repre¬ 
sent  the  large  majority  of  robots  used  in  industrial  settings.  However,  mobile 
robots  are  becoming  increasingly  important  in  advanced  applications,  in  view 
of  their  potential  for  autonomous  intervention.  This  chapter  presents  tech¬ 
niques  for  modelling,  planning  and  control  of  wheeled  mobile  robots.  The 
structure  of  the  kinematic  constraints  arising  from  the  pure  rolling  of  the 
wheels  is  first  analyzed;  it  is  shown  that  such  constraints  are  in  general  non- 
holonomic  and  consequently  reduce  the  local  mobility  of  the  robot.  The  kine¬ 
matic  model  associated  with  the  constraints  is  introduced  to  describe  the 
instantaneous  admissible  motions,  and  conditions  are  given  under  which  it 
can  be  put  in  chained  form.  The  dynamic  model,  that  relates  the  admissible 
motions  to  the  generalized  forces  acting  on  the  robot  DOFs,  is  then  derived. 
The  peculiar  nature  of  the  kinematic  model,  and  in  particular  the  existence  of 
flat  outputs,  is  exploited  to  devise  trajectory  planning  methods  that  guarantee 
that  the  nonholonomic  constraints  are  satisfied.  The  structure  of  minimum¬ 
time  trajectories  is  also  analyzed.  The  motion  control  problem  for  mobile 
robots  is  then  discussed,  with  reference  to  two  basic  motion  tasks,  i.e.,  trajec¬ 
tory  tracking  and  posture  regulation.  The  chapter  concludes  by  surveying  some 
techniques  for  odometric  localization  that  is  necessary  to  implement  feedback 
control  schemes. 


11.1  Nonholonomic  Constraints 

Wheels  are  by  far  the  most  common  mechanism  to  achieve  locomotion  in 
mobile  robots.  Any  wheeled  vehicle  is  subject  to  kinematic  constraints  that 
reduce  in  general  its  local  mobility,  while  leaving  intact  the  possibility  of 
reaching  arbitrary  configurations  by  appropriate  manoeuvres.  For  example, 
any  driver  knows  by  experience  that,  while  it  is  impossible  to  move  instanta¬ 
neously  a  car  in  the  direction  orthogonal  to  its  heading,  it  is  still  possible  to 
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park  it  arbitrarily,  at  least  in  the  absence  of  obstacles.  It  is  therefore  important 
to  analyze  in  detail  the  structure  of  these  constraints. 

In  accordance  with  the  terminology  introduced  in  Sect.  B.4,  consider  a 
mechanical  system  whose  configuration  q  £  C  is  described  by  a  vector  of 
generalized  coordinates ,  and  assume  that  the  configuration  space  C  (i.e. ,  the 
space  of  all  possible  robot  configurations)  coincides1  with  IR".  The  motion  of 
the  system  that  is  represented  by  the  evolution  of  q  over  time  may  be  sub¬ 
ject  to  constraints  that  can  be  classified  under  various  criteria.  For  example, 
they  may  be  expressed  as  equalities  or  inequalities  (respectively,  bilateral  or 
unilateral  constraints),  and  they  may  depend  explicitly  on  time  or  not  ( rheo - 
nomic  or  scleronomic  constraints) .  In  this  chapter,  only  bilateral  scleronomic 
constraints  will  be  considered. 

Constraints  that  can  be  put  in  the  form 

hi(q)  =  0  i  =  1, . . . ,  k  <  n  (11.1) 

are  called  holonomic  (or  integrable).  In  the  following,  it  is  assumed  that  the 
functions  hi  :  C  i— >  IR  are  of  class  C°°  ( smooth )  and  independent.  The  effect 
of  holonomic  constraints  is  to  reduce  the  space  of  accessible  configurations  to 
a  subset  of  C  with  dimension  n  ~  k.  A  mechanical  system  for  which  all  the 
constraints  can  be  expressed  in  the  form  (11.1)  is  called  holonomic. 

In  the  presence  of  holonomic  constraints,  the  implicit  function  theorem  can 
be  used  in  principle  to  solve  the  equations  in  (11.1)  by  expressing  k  generalized 
coordinates  as  a  function  of  the  remaining  n  —  k,  so  as  to  eliminate  them  from 
the  formulation  of  the  problem.  However,  in  general  this  procedure  is  only 
valid  locally,  and  may  introduce  singularities.  A  convenient  alternative  is  to 
replace  the  original  generalized  coordinates  with  a  reduced  set  of  n  —  k  new 
coordinates  that  are  directly  defined  on  the  accessible  subspace,  in  such  a 
way  that  the  available  DOFs  are  effectively  characterized.  The  mobility  of  the 
reduced  system  thus  obtained  is  completely  equivalent  to  that  of  the  original 
mechanism. 

Holonomic  constraints  are  generally  the  result  of  mechanical  interconnec¬ 
tions  between  the  various  bodies  of  the  system.  For  example,  prismatic  and 
revolute  joints  used  in  robot  manipulators  are  a  typical  source  of  such  con¬ 
straints,  and  joint  variables  are  an  example  of  reduced  sets  of  coordinates  in 
the  above  sense.  Constraints  of  the  form  (11.1)  may  also  arise  in  particular 
operating  conditions;  for  example,  one  may  mention  the  case  of  a  kinemati¬ 
cally  redundant  manipulator  that  moves  while  keeping  the  end-effector  fixed 
at  a  certain  pose  ( self-motion ). 

Constraints  that  involve  generalized  coordinates  and  velocities 
afiq,  q)  =  0  i  =  l,...,k<n 

1  This  assumption  is  taken  for  simplicity.  In  the  general  case,  the  configuration 
space  C  may  be  identified  with  a  Euclidean  space  only  on  a  local  basis,  because 
its  global  geometric  structure  is  more  complex;  this  will  be  further  discussed  in 
Chap.  12.  The  material  presented  in  this  chapter  is,  however,  still  valid. 
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are  called  kinematic.  They  constrain  the  instantaneous  admissible  motion  of 
the  mechanical  system  by  reducing  the  set  of  generalized  velocities  that  can  be 
attained  at  each  configuration.  Kinematic  constraints  are  generally  expressed 
in  Pfaffian  form,  i.e.,  they  are  linear  in  the  generalized  velocities: 

af(<l)Q  =  0  i  =  1, . . . ,  k  <  n,  (11.2) 


or,  in  matrix  form 

AT{q)q=  0.  (11.3) 

Vectors  a*  :  C  i— >  IR"  are  assumed  to  be  smooth  as  well  as  linearly  independent. 

Clearly,  the  existence  of  k  holonomic  constraints  (11.1)  implies  that  of  an 
equal  number  of  kinematic  constraints: 


dhj(q)  _  dhj(q)  .  _ 
dt  dq  q 


i  =  1, . . . ,  k. 


However,  the  converse  is  not  true  in  general.  A  system  of  kinematic  constraints 
in  the  form  (11.3)  may  or  may  not  be  integrable  to  the  form  (11.1).  In  the 
negative  case,  the  kinematic  constraints  are  said  to  be  nonholonomic  (or  non- 
integrable).  A  mechanical  system  that  is  subject  to  at  least  one  such  constraint 
is  called  nonholonomic. 

Nonholonomic  constraints  reduce  the  mobility  of  the  mechanical  system 
in  a  completely  different  way  with  respect  to  holonomic  constraints.  To  ap¬ 
preciate  this  fact,  consider  a  single  Pfaffian  constraint 


aT(q)q  =  0. 


(11.4) 


If  the  constraint  is  holonomic,  it  can  be  integrated  and  written  as 

h(q)  =  c,  (11.5) 

where  dh/dq  =  y(q)  aT (q),  with  7 (q)  ^  0  an  integrating  factor  and  c  an 
integration  constant.  Therefore,  there  is  a  loss  of  accessibility  in  the  configu¬ 
ration  space,  because  the  motion  of  the  mechanical  system  in  C  is  confined  to 
a  particular  level  surface  of  the  scalar  function  h.  This  surface,  which  depends 
on  the  initial  configuration  qQ  through  the  value  of  h(q0)  =  c,  has  dimension 
n  —  1. 

Assume  instead  that  the  constraint  (11.4)  is  nonholonomic.  In  this  case, 
generalized  velocities  are  indeed  constrained  to  belong  to  a  subspace  of  di¬ 
mension  n—  1,  i.e.,  the  null  space  of  matrix  aT{q).  Nevertheless,  the  fact  that 
the  constraint  is  non-integrable  means  that  there  is  no  loss  of  accessibility 
in  C  for  the  system.  In  other  words,  while  the  number  of  DOFs  decreases  to 
n  —  1  due  to  the  constraint,  the  number  of  generalized  coordinates  cannot  be 
reduced,  not  even  locally. 

The  conclusion  just  drawn  for  the  case  of  a  single  constraint  is  general. 
An  n-dimensional  mechanical  system  subject  to  k  nonholonomic  constraints 
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Fig.  11.1.  Generalized  coordinates  for  a  disk  rolling  on  a  plane 


can  access  its  whole  configuration  space  C,  although  at  any  configuration  its 
generalized  velocities  must  belong  to  an  (n  —  fc)-diinensional  subspace. 

The  following  is  a  classical  example  of  nonholonomic  mechanical  system, 
that  is  particularly  relevant  in  the  study  of  mobile  robots. 


Example  11.1 

Consider  a  disk  that  rolls  without  slipping  on  the  horizontal  plane,  while  keeping 
its  sagittal  plane  (i.e.,  the  plane  that  contains  the  disk)  in  the  vertical  direction 
(Fig.  11.1).  Its  configuration  is  described  by  three2  generalized  coordinates:  the 
Cartesian  coordinates  ( x ,  y)  of  the  contact  point  with  the  ground,  measured  in  a 
fixed  reference  frame,  and  the  angle  #  characterizing  the  orientation  of  the  disk  with 
respect  to  the  x  axis.  The  configuration  vector  is  therefore  q  =  [x  y  9]T . 

The  pure  rolling  constraint  for  the  disk  is  expressed  in  the  Pfaffian  form  as 

Asm#  —  ycos9  =  [sin#  —cos#  0]q  =  0,  (11.6) 

and  entails  that,  in  the  absence  of  slipping,  the  velocity  of  the  contact  point  has  zero 
component  in  the  direction  orthogonal  to  the  sagittal  plane.  The  angular  velocity  of 
the  disk  around  the  vertical  axis  instead  is  unconstrained. 

Constraint  (11.6)  is  nonholonomic,  because  it  implies  no  loss  of  accessibility  in 
the  configuration  space  of  the  disk.  To  substantiate  this  claim,  consider  that  the 
disk  can  be  driven  from  any  initial  configuration  qt  =  [xi  yi  #;  ]T  to  any  final 
configuration  =  [x/  yg  #/]T  through  the  following  sequence  of  movements 
that  do  not  violate  constraint  (11.6): 

1.  rotate  the  disk  around  its  vertical  axis  so  as  to  reach  the  orientation  #„  for  which 
the  sagittal  axis  (i.e.,  the  intersection  of  the  sagittal  plane  and  the  horizontal 
plane)  goes  through  the  final  contact  point  (*/,?//); 

2.  roll  the  disk  on  the  plane  at  a  constant  orientation  #„  until  the  contact  point 
reaches  its  final  position  (x/,y/); 

3.  rotate  again  the  disk  around  its  vertical  axis  to  change  the  orientation  from  #„ 
to  Of. 

2  One  could  add  to  this  description  an  angle  <f>  measuring  the  rotation  of  the  disk 
around  the  horizontal  axis  passing  through  its  centre.  Such  a  coordinate  is  however 
irrelevant  for  the  analysis  presented  in  this  chapter,  and  is  therefore  ignored  in 
the  following. 
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Fig.  11.2.  A  local  representation  of  the  configuration  space  for  the  rolling  disk  with 
an  example  manoeuvre  that  transfers  the  configuration  from  qi  to  Qj  ( dashed  line) 


An  example  of  this  manoeuvre  is  shown  in  Fig.  11.2.  Two  possible  directions  of 
instantaneous  motion  are  shown  at  each  configuration:  the  first,  that  is  aligned  with 
the  sagittal  axis,  moves  the  contact  point  while  keeping  the  orientation  constant 
(rolling);  the  second  varies  the  orientation  while  keeping  the  contact  point  fixed 
(rotation  around  the  vertical  axis). 


It  is  interesting  to  note  that,  in  addition  to  wheeled  vehicles,  there  exist 
other  robotic  systems  that  are  nonholonomic  in  nature.  For  example,  the  pure 
rolling  constraint  also  arises  in  manipulation  problems  with  round-fingered 
robot  hands.  Another  kind  of  nonholonomic  behaviour  is  found  in  multibody 
systems  that  ‘float’  freely  (i.e.,  without  a  fixed  base),  such  as  manipulators 
used  in  space  operations.  In  fact,  in  the  absence  of  external  generalized  forces, 
the  conservation  of  the  angular  momentum  represents  a  non-integrable  Pfaf- 
fian  constraint  for  the  system. 


11.1.1  Integrability  Conditions 

In  the  presence  of  Pfaffian  kinematic  constraints,  integrability  conditions  can 
be  used  to  decide  whether  the  system  is  holonomic  or  nonholonomic. 
Consider  first  the  case  of  a  single  Pfaffian  constraint: 

n 

aT(q)q  =  ^  a^q)^  =  0.  (11.7) 

i= i 

For  this  constraint  to  be  integrable,  there  must  exist  a  scalar  function  h(q) 
and  an  integrating  factor  7 (<7)  ^  0  such  that  the  following  condition  holds: 

dh(q) 
dqj 


7 (q)aj(q) 


j  =  !,•••,  n. 


(11.8) 
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The  converse  is  also  true:  if  there  exists  an  integrating  factor  y (q)  ^  0  such 
that  7 (q)a(q)  is  the  gradient  of  a  scalar  function  h(q),  constraint  (11.7)  is 
integrable.  By  using  Schwarz  theorem  on  the  symmetry  of  second  derivatives, 
the  integrability  condition  (11.8)  may  be  replaced  by  the  following  system  of 
partial  differential  equations: 


<9(7 a-k)  _  d(j dj) 

dq3  dqk 


j,k  =  l,...,n,  j^k, 


(11.9) 


that  does  not  contain  the  unknown  function  h(q).  Note  that  condition  (11.9) 
implies  that  a  Pfaffian  constraint  with  constant  coefficients  a3  is  always  holo- 
nomic. 


Example  11.2 

Consider  the  following  kinematic  constraint  in  C  =  1R3: 


<?i  +  Qi<?2  +  q3  =  0. 
The  holonomy  condition  (11.9)  gives 

dy 
dq2 
dy 

d<?3 

dy 


ITT  =  7  +  ?i  w- 


dy 

dqi 


q  1 


dq-i 


dy 

dqi 

dy 

d<?2 ' 


By  substituting  the  second  and  third  equations  into  the  first,  it  is  easy  to  conclude 
that  the  only  solution  is  7  =  0.  Therefore,  the  constraint  is  nonholonomic. 


Example  11.3 


Consider  the  pure  rolling  constraint  (11.6).  In  this  case,  the  holonomy  condi¬ 
tion  (11.9)  gives 


dy  dy 

sinf/—  =  —COS  0  -r— 

ay  ox 

dy 

cos  ^  wx  —  7  sin  9 
06 

dy 

sin#  —  =  — ycos#. 
o9 


Squaring  and  adding  the  last  two  equations  gives  dy/d#  =  ±7.  Assume  for  example 
dy /d0  =  y.  Using  this  in  the  above  equations  leads  to 


y  cos  9  —  y  sin  9 
7  sin#  =  — ycos# 

whose  only  solution  is  y  =  0.  The  same  conclusion  is  reached  by  letting  dy/d#  =  —7. 
This  confirms  that  constraint  (11.6)  is  nonholonomic. 
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The  situation  becomes  more  complicated  when  dealing  with  a  system  of 
k  >  1  kinematic  constraints  of  the  form  (11.3).  In  fact,  in  this  case  it  may 
happen  that  the  single  constraints  are  not  integrable  if  taken  separately,  but 
the  whole  system  is  integrable.  In  particular,  if  p  <  k  independent  linear 
combinations  of  the  constraints 
k 

{q)q  j  =  l,...,p 

i= 1 

are  integrable,  there  exist  p  independent  scalar  functions  hj(q)  such  that 

dhi{q)  dhp(q)  1  ,  T  T  , 

dq  ’ '  dq  J  c  sPan  lai  •  •  • ,  afc  (<?)}  Vq  €  C. 

Therefore,  the  configurations  that  are  accessible  for  the  mechanical  system 
belong  to  the  (n  —  p)-dimensional  subspace  consisting  of  the  particular  level 
surfaces  of  the  functions  hf. 

{q  €  C:  hi(q)  =  a, . . . ,  hp(q)  =  cp} 

on  which  the  motion  is  started  (see  Problem  11.2).  In  the  case  p  =  k,  the 
system  of  kinematic  constraints  (11-3)  is  completely  integrable,  and  hence 
holonomic. 


span 


Example  11.4 

Consider  the  system  of  Pfafhan  constraints 

qi  +  <7i<22  +  ?3  =  0 
qi  +  <72  +  qiq3  =  0. 

Taken  separately,  these  constraints  are  found  to  be  non-integrable  (in  particular, 
the  first  is  the  nonholonomic  constraint  of  Example  11.2).  However,  subtracting  the 
second  from  the  first  gives 

(qi  ~  1)(?2  —  <73)  =  0 

so  that  q2  =  q 3,  because  the  constraints  must  be  satisfied  for  any  value  of  q.  The 
assigned  system  of  constraints  is  then  equivalent  to 

d2  =  q  3 
<71  +  (1  +  <?i)<72  =  0, 

which  can  be  integrated  as 


<72  —  ?3  =  Cl 
log(<7i  +  1)  +  172  =  c2 


with  integration  constants  ci,  C2. 
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The  integrability  conditions  of  a  system  of  Pfaffian  kinematic  constraints 
are  quite  complex  and  derive  from  a  fundamental  result  of  differential  geome¬ 
try  known  as  Frobenius  theorem.  However,  as  shown  later  in  the  chapter,  it  is 
possible  to  derive  such  conditions  more  directly  from  a  different  perspective. 


11.2  Kinematic  Model 

The  system  of  k  Pfaffian  constraints  (11.3)  entails  that  the  admissible  gener¬ 
alized  velocities  at  each  configuration  q  belong  to  the  (n  —  fc)-dimensional 
null  space  of  matrix  AT(q).  Denoting  by  { gx(q), . . .  ,gn_k(q)}  a  basis  of 
Af(AT(q)),  the  admissible  trajectories  for  the  mechanical  system  can  then 
be  characterized  as  the  solutions  of  the  nonlinear  dynamic  system 

m 

q  =  '^2gj(q)uj  =  G(q)u  m  =  n  -  k,  (11.10) 

j= i 

where  q  G  IR"  is  the  state  vector  and  u  =  [ui  ...  um  ]T  €  Rm  is  the  input 
vector.  System  (11.10)  is  said  to  be  driftless  because  one  has  q  =  0  if  the 
input  is  zero. 

The  choice  of  the  input  vector  fields  gi(q),  ■  ■  ■  ,gm(q)  (and  thus  of  ma¬ 
trix  G(q))  in  (11.10)  is  not  unique.  Correspondingly,  the  components  of  u 
may  have  different  meanings.  In  general,  it  is  possible  to  choose  the  basis  of 
A f(AT(q))  in  such  a  way  that  the  UjS  have  a  physical  interpretation,  as  will 
be  shown  later  for  some  examples  of  mobile  robots.  In  any  case,  vector  u  may 
not  be  directly  related  to  the  actual  control  inputs,  that  are  in  general  forces 
and/or  torques.  For  this  reason,  Eq.  (11.10)  is  referred  to  as  the  kinematic 
model  of  the  constrained  mechanical  system. 

The  holonomy  or  nonholonomy  of  constraints  (11-3)  can  be  established 
by  analyzing  the  controllability3  properties  of  the  associated  kinematic  model 
(11.10).  In  fact,  two  cases  are  possible: 

1.  If  system  (11.10)  is  controllable,  given  two  arbitrary  configurations  qi  and 
qf  in  C,  there  exists  a  choice  of  u(t)  that  steers  the  system  from  q,L  to  qj, 
i.e. ,  there  exists  a  trajectory  that  joins  the  two  configurations  and  satisfies 
the  kinematic  constraints  (11.3).  Therefore,  these  do  not  affect  in  any  way 
the  accessibility  of  C,  and  they  are  (completely)  nonholonomic. 

2.  If  system  (11.10)  is  not  controllable,  the  kinematic  constraints  (11-3)  re¬ 
duce  the  set  of  accessible  configurations  in  C.  Hence,  the  constraints  are 
partially  or  completely  integrable  depending  on  the  dimension  v  <  n  of 
the  accessible  configuration  space.  In  particular: 


3  Refer  to  Appendix  D  for  a  short  survey  of  nonlinear  controllability  theory,  in¬ 
cluding  the  necessary  tools  from  differential  geometry. 
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2a.  If  m  <  v  <  n,  the  loss  of  accessibility  is  not  maximal,  and  thus  con¬ 
straints  (11.3)  are  only  partially  integrable.  The  mechanical  system  is 
still  nonholonomic. 

2b.  If  v  =  m,  the  loss  of  accessibility  is  maximal,  and  constraints  (11.3)  are 
completely  integrable.  Therefore,  the  mechanical  system  is  holonomic. 

Note  how  this  particular  viewpoint,  i.e. ,  the  equivalence  between  control¬ 
lability  and  nonholonomy,  was  already  implicitly  adopted  in  Example  11.1, 
where  the  controllability  of  the  kinematic  system  was  proven  constructively , 
i.e.,  by  exhibiting  a  reconfiguration  manoeuvre.  A  more  systematic  approach 
is  to  take  advantage  of  the  controllability  conditions  for  nonlinear  driftless 
systems.  In  particular,  controllability  may  be  verified  using  the  accessibility 
rank  condition 

dim  A^(q)  =  n,  (11.11) 

where  4  is  the  accessibility  distribution  associated  with  system  (11.10),  i.e., 
the  involutive  closure  of  distribution  A  —  span {g1: . . . ,  gm}.  The  following 
cases  may  occur: 

1.  If  (11.11)  holds,  system  (11.10)  is  controllable  and  the  kinematic  con¬ 
straints  (11-3)  are  (completely)  nonholonomic. 

2.  If  (11.11)  does  not  hold,  system  (11.10)  is  not  controllable  and  the  kine¬ 
matic  constraints  (11-3)  are  at  least  partially  integrable.  In  particular,  let 

dimZ\_4(q)  =  v  <  n. 


Then 

2a.  If  to  <  v  <  n,  constraints  (11.3)  are  only  partially  integrable. 

2b.  If  v  =  to,  constraints  (11.3)  are  completely  integrable,  and  hence  holo¬ 
nomic.  This  happens  when  A coincides  with  A  =  spanjgq, . . .  ,gm}, 
i.e.,  when  the  latter  distribution  is  involutive. 

It  is  easy  to  verify  that,  in  the  case  of  a  single  kinematic  constraint  (11.7), 
the  integrability  condition  given  by  (11.9)  is  equivalent  to  the  involutivity  of 
A  =  spanjgq, . . . ,  <7n_i}-  Another  remarkable  situation  is  met  when  the  num¬ 
ber  of  Pfaffian  constraints  is  k  =  n  —  1;  in  this  case,  the  associated  kinematic 
model  (11.10)  consists  of  a  single  vector  field  g  (to  =  1).  Hence,  n  —  1  Pfaffian 
constraints  are  always  integrable,  because  the  distribution  associated  with  a 
single  vector  field  is  always  involutive.  For  example,  a  mechanical  system  with 
two  generalized  coordinates  that  is  subject  to  a  scalar  Pfaffian  constraint  is 
always  holonomic. 

In  the  following,  the  kinematic  models  of  two  wheeled  vehicles  of  particular 
interest  will  be  analyzed  in  detail.  A  large  part  of  the  existing  mobile  robots 
have  a  kinematic  model  that  is  equivalent  to  one  of  these  two. 
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Fig.  11.3.  Generalized  coordinates  for  a  unicycle 


11.2.1  Unicycle 

A  unicycle  is  a  vehicle  with  a  single  orientable  wheel.  Its  configuration  is 
completely  described  by  q  =  [x  y  0}T ,  where  (x ,y)  are  the  Cartesian  co¬ 
ordinates  of  the  contact  point  of  the  wheel  with  the  ground  (or  equivalently, 
of  the  wheel  centre)  and  6  is  the  orientation  of  the  wheel  with  respect  to  the 
x  axis  (see  Fig.  11.3). 

As  already  seen  in  Example  11.1,  the  pure  rolling  constraint  for  the  wheel 
is  expressed  as 


isin0  —  ycosd  =  [sin  6*  —cos  8  0]q  =  0,  (11.12) 


entailing  that  the  velocity  of  the  contact  point  is  zero  in  the  direction  orthog¬ 
onal  to  the  sagittal  axis  of  the  vehicle.  The  line  passing  through  the  contact 
point  and  having  such  direction  is  therefore  called  zero  motion  line.  Consider 
the  matrix 


G{q)  =  [g1{q)  32(<?)] 


cos  6  0 

sin  6  0 

0  1 


whose  columns  g1(q)  and  g2{q)  are,  for  each  q ,  a  basis  of  the  null  space  of  the 
matrix  associated  with  the  Pfaffian  constraint.  All  the  admissible  generalized 
velocities  at  q  are  therefore  obtained  as  a  linear  combination  of  g±{q)  and 
g2(q).  The  kinematic  model  of  the  unicycle  is  then 


X 

cos  6 

'o' 

y 

= 

sin  6 

v  + 

0 

_e_ 

0 

1 

(11.13) 


where  the  inputs  v  and  lo  have  a  clear  physical  interpretation.  In  particular, 
v  is  the  driving  velocity,  i.e.,  the  modulus4  (with  sign)  of  the  contact  point 

4  Note  that  v  is  given  by  the  angular  speed  of  the  wheel  around  its  horizontal  axis 
multiplied  by  the  wheel  radius. 
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velocity  vector,  whereas  the  steering  velocity  to  is  the  wheel  angular  speed 
around  the  vertical  axis. 

The  Lie  bracket  of  the  two  input  vector  fields  is 


[fluff  2]  (9) 


sin  6 
—cos  9 
0 


that  is  always  linearly  independent  from  g1(q),g2(q).  Therefore,  the  iterative 
procedure  (see  Sect.  D.2)  for  building  the  accessibility  distribution  Z\_q  ends 
with 

dim Z\_4  =  dim Z\2  =  dimspanj^,^,  [g1,g2]}  =  3. 

This  indicates  that  the  unicycle  is  controllable  with  degree  of  nonholonomy 
k  =  2,  and  that  constraint  (11.12)  is  nonholonomic  —  the  same  conclusion 
reached  in  Example  11.3  by  applying  the  integrability  condition. 

A  unicycle  in  the  strict  sense  (i.e.,  a  vehicle  equipped  with  a  single  wheel) 
is  a  robot  with  a  serious  problem  of  balance  in  static  conditions.  However, 
there  exist  vehicles  that  are  kinematically  equivalent  to  a  unicycle  but  more 
stable  from  a  mechanical  viewpoint.  Among  these,  the  most  important  are 
the  differential  drive  and  the  synchro  drive  vehicles,  already  introduced  in 
Sect.  1.2.2. 

For  the  differential  drive  mobile  robot  of  Fig.  1.13,  denote  by  ( x,y )  the 
Cartesian  coordinates  of  the  midpoint  of  the  segment  joining  the  two  wheel 
centres,  and  by  8  the  common  orientation  of  the  fixed  wheels  (hence,  of  the 
vehicle  body).  Then,  the  kinematic  model  (11.13)  of  the  unicycle  also  applies 
to  the  differential  drive  vehicle,  provided  that  the  driving  and  steering  veloc¬ 
ities  v  and  lo  are  expressed  as  a  function  of  the  actual  velocity  inputs,  i.e., 
the  angular  speeds  u>R  and  of  the  right  and  left  wheel,  respectively.  Simple 
arguments  (see  Problem  11.6)  can  be  used  to  show  that  there  is  a  one-to-one 
correspondence  between  the  two  sets  of  inputs: 


r  ( ojr  +  uL) 
2 


UJ  = 


r  (u>R  -  ojl) 

d 


(11.14) 


where  r  is  the  radius  of  the  wheels  and  d  is  the  distance  between  their  centres. 

The  equivalence  with  the  kinematic  model  (11.13)  is  even  more  straight¬ 
forward  for  the  synchro  drive  mobile  robot  of  Fig.  1.14,  whose  control  inputs 
are  indeed  the  driving  velocity  v  and  the  steering  velocity  to,  that  are  common 
to  the  three  orientable  wheels.  The  Cartesian  coordinates  ( x ,  y)  may  repre¬ 
sent  in  this  case  any  point  of  the  robot  (for  example,  its  centroid),  while  0  is 
the  common  orientation  of  the  wheels.  Note  that,  unlike  a  differential  drive 
vehicle,  the  orientation  of  the  body  of  a  synchro  drive  vehicle  never  changes, 
unless  a  third  actuator  is  added  for  this  specific  purpose. 


11.2.2  Bicycle 

Consider  now  a  bicycle,  i.e.,  a  vehicle  having  an  orientable  wheel  and  a  fixed 
wheel  arranged  as  in  Fig.  11.4.  A  possible  choice  for  the  generalized  coordi- 
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Fig.  11.4.  Generalized  coordinates  and  instantaneous  centre  of  rotation  for  a  bicycle 


nates  is  q  =  [x  y  9  (j)]T,  where  (x,  y)  are  the  Cartesian  coordinates  of  the 

contact  point  between  the  rear  wheel  and  the  ground  (i.e. ,  of  the  rear  wheel 
centre),  6  is  the  orientation  of  the  vehicle  with  respect  to  the  x  axis,  and  cj>  is 
the  steering  angle  of  the  front  wheel  with  respect  to  the  vehicle. 

The  motion  of  the  vehicle  is  subject  to  two  pure  rolling  constraints,  one 
for  each  wheel: 


Xf  sin  (9  +  4>)  —  yf  cos  (9  +  <j>)  =  0  (11.15) 

xsm9  —  ycos9  =  0,  (11.16) 

where  (xf,yf)  is  the  Cartesian  position  of  the  centre  of  the  front  wheel.  The 
geometric  meaning  of  these  constraints  is  obvious:  the  velocity  of  the  centre  of 
the  front  wheel  is  zero  in  the  direction  orthogonal  to  the  wheel  itself,  while  the 
velocity  of  the  centre  of  the  rear  wheel  is  zero  in  the  direction  orthogonal  to 
the  sagittal  axis  of  the  vehicle.  The  zero  motion  lines  of  the  two  wheels  meet 
at  a  point  C  called  instantaneous  centre  of  rotation  (Fig.  11.4),  whose  position 
depends  only  on  (and  changes  with)  the  configuration  q  of  the  bicycle.  Each 
point  of  the  vehicle  body  then  moves  instantaneously  along  an  arc  of  circle 
with  centre  in  C  (see  also  Problem  11.7). 

Using  the  rigid  body  constraint 

Xf  =  x  +  l  cos  9 
Vf  =  y  +  t  sin  9, 

where  £  is  the  distance  between  the  wheels,  constraint  (11.15)  can  be  rewritten 
as 

a;  sin  (9  +  cf)  —  y  cos  (9  +  4>)  —  £9  cos<j)  =  0.  (11.17) 

The  matrix  associated  with  the  Pfaffian  constraints  (11.16),  (11.17)  is  then 

sin  9  —cos  9  0  0 

sin  ( 9  +  4>)  —cos  (9  +  <f>)  —t  cos  (j>  0  ’ 


AT(q)  = 
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with  constant  rank  k  =  2.  The  dimension  of  its  null  space  is  n  —  k  =  2,  and 
all  the  admissible  velocities  at  q  may  be  written  as  a  linear  combination  of  a 
basis  of  A f(AT(q)),  for  example 


X 

"  cos  6  cos  q i ' 

-0- 

y 

sin  9  cos  <t> 

Ml  + 

0 

0 

sin  (j>/£ 

0 

A. 

0 

.1. 

Since  the  front  wheel  is  orientable,  it  is  immediate  to  set  u2  =  u>,  where  u>  is 
the  steering  velocity.  The  expression  of  u\  depends  instead  on  how  the  vehicle 
is  driven. 

If  the  bicycle  has  front-wheel  drive,  one  has  directly  U\  =  v,  where  v  is  the 
driving  velocity  of  the  front  wheel.  The  corresponding  kinematic  model  is 


X 

" cosdcosq i" 

-o- 

y 

sin  9  cos  (j> 

0 

9 

sin  (j)/l 

v  + 

0 

<t>. 

0 

.1. 

(11.18) 


Denoting  by  g1(q)  and  g2(q )  the  two  input  vector  fields,  simple  computations 
give 


"  cos  9  sin  (f> " 

'  —  sin  9/1' 

S3(g)  =  [9i,92\(q)  = 

sin  9  sin  <f 
—cos  <§>jl 

9M )  =  [9i,93}(9)  = 

cos 9/t 

0 

0 

0  . 

both  linearly  independent  from  gi{q)  and  g2(q).  Hence,  the  iterative  proce¬ 
dure  for  building  the  accessibility  distribution  Z\_q  ends  with 

dim  Z\_4  =  dim  A3  =  dimspan{g1,g2,g3,g4}  =  4. 


This  means  that  the  front-wheel  drive  bicycle  is  controllable  with  degree  of 
nonholonomy  n  =  3,  and  constraints  (11.15),  (11.16)  are  (completely)  non- 
holonomic. 

The  kinematic  model  of  a  bicycle  with  rear-wheel  drive  can  be  derived  by 
noting  that  in  this  case  the  first  two  equations  must  coincide  with  those  of 
the  unicycle  model  (11.13).  It  is  then  sufficient  to  set  u\  =  v/cos<p  to  obtain 


X 

cos  9 

-o- 

y 

sin  9 

v  + 

0 

9 

tancf/i 

0 

A. 

.  0  . 

.1. 

(11.19) 
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where  v  is  the  driving  velocity  of  the  rear  wheel.5  In  this  case,  one  has 


0 

0 

1 

sin  9  - 

03(<z)  =  [0i,02](g)  = 

34(g)  =  [ffi,  03]  (?)  = 

icos  2<j) 
cos  9 
icos  2(j) 

0 

0 

£cos  2(j) 

0 

again  linearly  independent  from  g1(q)  and  92(9)-  Hence,  the  rear-wheel  drive 
bicycle  is  also  controllable  with  degree  of  nonholonomy  n  =  3. 

Like  the  unicycle,  the  bicycle  is  also  unstable  in  static  conditions.  Kine¬ 
matically  equivalent  vehicles  that  are  mechanically  balanced  are  the  tricycle 
and  the  car-like  robot,  introduced  in  Sect.  1.2.2  and  shown  respectively  in 
Fig.  1.15  and  1.16.  In  both  cases,  the  kinematic  model  is  given  by  (11.18)  or 
by  (11.19)  depending  on  the  wheel  drive  being  on  the  front  or  the  rear  wheels. 
In  particular,  ( x ,  y)  are  the  Cartesian  coordinates  of  the  midpoint  of  the  rear 
wheel  axle,  9  is  the  orientation  of  the  vehicle,  and  <f>  is  the  steering  angle. 


11.3  Chained  Form 

The  possibility  of  transforming  the  kinematic  model  (11.10)  of  a  mobile  robot 
in  a  canonical  form  is  of  great  interest  for  solving  planning  and  control  prob¬ 
lems  with  efficient,  systematic  procedures.  Here,  the  analysis  is  limited  to 
systems  with  two  inputs,  like  the  unicycle  and  bicycle  models. 

A  (2,  n)  chained  form  is  a  two-input  driftless  system 

z  =  h{z)vi  +  l2{z)v2, 

whose  equations  are  expressed  as 

Zi  =  Vi 

z2  =  v2 

Z3  =  z2vi  (11.20) 

zn  =  Zn-  1U1. 

Using  the  following  notation  for  a  ‘repeated’  Lie  bracket: 

ad7l72  =  [7i,72]  ad7l72  =  [7i,  ad*“172], 

5  Note  that  the  kinematic  model  (11.19)  is  no  longer  valid  for  (j>  =  ±7t/2,  where  the 
first  vector  field  is  not  defined.  This  corresponds  to  the  mechanical  jam  in  which 
the  front  wheel  is  orthogonal  to  the  sagittal  axis  of  the  vehicle.  This  singularity 
does  not  arise  in  the  front-wheel  drive  bicycle  (11.18),  that  in  principle  can  still 
pivot  around  the  rear  wheel  contact  point  in  such  a  situation. 
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one  has  for  system  (11.20) 


'  1  ' 

"O' 

'  0  ' 

0 

1 

7i  = 

^2 

23 

72  = 

0 

0 

=>  ad7i72  — 

(-l)fe 

-  Zn—  1  - 

.0. 

.  0  . 

where  (— l)fe  is  the  (k  +  2)-th  component.  This  implies  that  the  system  is 
controllable,  because  the  accessibility  distribution 

Aa  =  span  {7r,72,  ad7i72, . . . ,  ad^272} 

has  dimension  n.  In  particular,  the  degree  of  nonholonomy  is  k  =  n  —  1. 

There  exist  necessary  and  sufficient  conditions  for  transforming  a  generic 
two-input  driftless  system 


Q  =  9i(q)ui  +g2(q)u2  (11.21) 

in  the  chained  form  (11.20)  via  coordinate  and  input  transformations 

z  =  T{q )  v  =  (3(q)u.  (11.22) 

In  particular,  it  can  be  shown  that  systems  like  (11.21)  with  dimension  n  not 
larger  than  4  can  always  be  put  in  chained  form.  This  applies,  for  example, 
to  the  kinematic  models  of  the  unicycle  and  the  bicycle. 

There  also  exist  sufficient  conditions  for  transformability  in  chained  form 
that  are  relevant  because  they  are  constructive.  Define  the  distributions 

A)  =  span  {g1,g2,a.dgig2, . . .  ,a,dg~2g2} 

A  =  span  {g2,adgig2,...,adg~2g2} 

A  =  span  {g2,ad9ig2,...,adg~3g2}. 

Assume  that,  in  a  certain  set,  it  is  dim  A  =  ni  A  and  A  are  involutive,  and 
there  exists  a  scalar  function  h\(q )  whose  differential  dh\  satisfies 


dh\  ■  A  =  0  dhi  ■  gl  =  1, 


where  the  symbol  •  denotes  the  inner  product  between  a  row  vector  and  a 
column  vector  —  in  particular,  -A  is  the  inner  product  with  any  vector 
generated  by  distribution  A  ■  In  this  case,  system  (11.21)  can  be  put  in  the 
form  (11.20)  through  the  coordinate  transformation6 

zi  =  hi 


This  transformation  makes  use  of  the  Lie  derivative  (see  Appendix  D). 
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*2  =  Lg~2fl2 

Zn—1  =  Lgih,2 
Zn  =  h2, 

where  h2  must  be  chosen  independent  of  hi  and  such  that  dh2  ■  A2  =  0.  The 
input  transformation  is  given  by 

Vi  =  Ml 

v2  =  (Lng~lh2)  ui  +  [Lg2L^-2h2)  u2. 

In  general,  the  coordinate  and  input  transformations  are  not  unique. 

Consider  the  kinematic  model  (11.13)  of  the  unicycle.  With  the  change  of 
coordinates 

z\  =  8 

z2  =  x  cos  6  +  y  sin  0 
Z3  =  x  sin  9  —  y  cos  9 

and  the  input  transformation 

V  =  V2  +  Z3V I 

u  =  Vi, 

one  obtains  the  (2,3)  chained  form 

h  =  Vi 

z2  =  v2  (11.25) 

Z3  =  Z2Vi. 

Note  that,  while  Z\  is  simply  the  orientation  0,  coordinates  z2  and  z3  represent 
the  position  of  the  unicycle  in  a  moving  reference  frame  whose  z2  axis  is  aligned 
with  the  sagittal  axis  of  the  vehicle  (see  Fig.  11.3). 

As  for  mobile  robots  with  bicycle-like  kinematics,  consider  for  example  the 
model  (11.19)  corresponding  to  the  rear-wheel  drive  case.  Using  the  change 
of  coordinates 

Z\  =  x 

1  ^ 

z2  =  -  sec  9  tan  cj) 
z3  =  tan  9 
Z4  =  V 


(11.23) 


(11.24) 
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and  the  input  transformation 
vi 

v  =  - a 

cos  9 


3  i 

u  =  —  -  iq  sec  0  sin  2  ft  +  j  v2  cos  3  9  cos  2(/>, 
the  (2,4)  chained  form  is  obtained: 


Zi  =  Vi 
Z2  =  V2 
Z 3  =  Z2V\ 
Z4  =  Z3Vi. 


This  transformation  is  defined  everywhere  in  the  configuration  space,  with  the 
exception  of  points  where  cos  0  =  0.  The  equivalence  between  the  two  models 
is  then  subject  to  the  condition  9  yf  ±kn/2,  with  k  =  1,2,...  . 


11.4  Dynamic  Model 

The  derivation  of  the  dynamic  model  of  a  mobile  robot  is  similar  to  the  ma¬ 
nipulator  case,  the  main  difference  being  the  presence  of  nonholonomic  con¬ 
straints  on  the  generalized  coordinates.  An  important  consequence  of  non- 
holonomy  is  that  exact  linearization  of  the  dynamic  model  via  feedback  is  no 
longer  possible.  In  the  following,  the  Lagrange  formulation  is  used  to  obtain 
the  dynamic  model  of  an  n-dimensional  mechanical  system  subject  to  k  <  n 
kinematic  constraints  in  the  form  (11.3),  and  it  is  shown  how  this  model  can 
be  partially  linearized  via  feedback. 

As  usual,  define  the  Lagrangian  C  of  the  mechanical  system  as  the  differ¬ 
ence  between  its  kinetic  and  potential  energy: 

£(q,g)  =  T(q,q)  -  U{q)  =  B(q)q  -  U{q),  (11.26) 

where  B(q)  is  the  (symmetric  and  positive  definite)  inertia  matrix  of  the 
mechanical  system.  The  Lagrange  equations  are  in  this  case 

UMY  -  = siq)r + Aiq)x'  (1L27) 

where  S(q)  is  an  (n  x  m)  matrix  mapping  the  m  =  n  —  k  external  inputs 
t  to  generalized  forces  performing  work  on  q ,  A(q)  is  the  transpose  of  the 
(k  x  n)  matrix  characterizing  the  kinematic  constraints  (11.3),  and  A  G  IRfc  is 
the  vector  of  Lagrange  multipliers.  The  term  A(q) A  represents  the  vector  of 
reaction  forces  at  the  generalized  coordinate  level.  It  has  been  assumed  that 
the  number  of  available  inputs  matches  the  number  of  DOFs  ( full  actuation), 
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that  is,  in  turn,  equal  to  the  number  n  of  generalized  coordinates  minus  the 
number  k  of  constraints. 

Using  (11.26),  (11.27),  the  dynamic  model  of  the  constrained  mechanical 
system  is  expressed  as 

B{q)q  +  n(q,q)  =  S(q)r  +  A{q)\  (11.28) 

AT(q)q  =  0,  (11.29) 


where 


Consider  now  a  matrix  G(q)  whose  columns  are  a  basis  for  the  null  space 
of  AT(q),  so  that  AT (q)G(q)  =  0.  One  can  replace  the  constraint  given 
by  (11.29)  with  the  kinematic  model 


q  =  G(q)v  =  '^2gi{q)vi,  (11.30) 

i= 1 

where  v  £  JRm  is  the  vector  of  pseudo-velocities’,’7  for  example,  in  the  case 
of  a  unicycle  the  components  of  this  vector  are  the  driving  velocity  v  and 
the  steering  velocity  ui.  Moreover,  the  Lagrange  multipliers  in  (11.28)  can  be 
eliminated  premultiplying  both  sides  of  the  equation  by  GT(q).  This  leads  to 
the  reduced  dynamic  model 

GT{q)  (B(q)q  +  n(q,  q))  =  GT(q)S(q)r,  (11.31) 

a  system  of  m  differential  equations. 

Differentiation  of  (11.30)  with  respect  to  time  gives 

q  =  G{q)v  +  G{q)v. 

Premultiplying  this  by  GT(q)B(q)  and  using  the  reduced  dynamic  model 
(11.31),  one  obtains 

M(q)v  +  m(q,  v)  =  GT (q)S(q)r,  (11.32) 


where 


M(q)  =  GT  (q)B(q)G(q) 
m(q,  v)  =  GT (q) B (q) G(q)v  +  GT(q)n(q ,  G(q)v), 


‘  In  the  dynamic  modeling  context,  the  use  of  this  term  emphasizes  the  difference 
between  v  and  q ,  that  are  the  actual  (generalized)  velocities  of  the  mechanical 
system. 
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with  M(q)  positive  definite  and 

G{q)v  =  G^)v- 

This  finally  leads  to  the  state-space  reduced  model 

q  =  G(q)v  (11.33) 

v  =  -M^1(q)m(q,v)  +  M-1  (q)GT  (q)  S'  (q)r ,  (11.34) 

that  represents  in  a  compact  form  the  kinematic  and  dynamic  models  of  the 
constrained  system  as  a  set  of  n  +  in  differential  equations. 

Suppose  now  that 

det  ( [GT(q)S(q ))  0, 

an  assumption  on  the  ‘control  availability’  that  is  satisfied  in  many  cases 
of  interest.  It  is  then  possible  to  perform  a  partial  linearization  via  feedback 
of  (11.33),  (11.34)  by  letting 

t  =  {GT{q)S(q)^j  (M(q)a  +  m(q,v)),  (11.35) 

where  a  £  lRm  is  the  pseudo-acceleration  vector.  The  resulting  system  is 

q  =  G(q)v  (11.36) 

v  =  a.  (11.37) 

Note  the  structure  of  this  system:  the  first  n  equations  are  the  kinematic 
model,  of  which  the  last  in  —  that  represent  the  inclusion  of  m  integrators  on 
the  input  channels  —  are  a  dynamic  extension.  If  the  system  is  unconstrained 
and  fully  actuated,  it  is  G{q)  =  S(q)  =  In ;  then,  the  feedback  law  (11.35) 
simply  reduces  to  an  inverse  dynamics  control  analogous  to  (8.57),  and  cor¬ 
respondingly  the  closed-loop  system  is  equivalent  to  n  decoupled  double  inte¬ 
grators. 

The  implementation  of  the  feedback  control  (11.35)  in  principle  requires 
the  measurement  of  v ,  and  this  may  not  be  available.  However,  pseudo¬ 
velocities  can  be  computed  via  the  kinematic  model  as 

v  =  G\q)q  =  {GT{q)G{q)y1  GT{q)q ,  (11.38) 

provided  that  q  and  q  are  measured.  Note  that  the  left  pseudo-inverse  of  G(q) 
has  been  used  here. 

By  defining  the  state  x  =  ( q,v )  £  IR"+m  and  the  input  u  =  a  £  IRm, 
system  (11.36),  (11.37)  can  be  expressed  as 

x  =  f(x)  +  G{x)u  =  G(q)v  +  0  ^  (11.39) 

U  -L  m 
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i.e.,  a  nonlinear  system  with  drift  also  known  as  the  second-order  kinematic 
model  of  the  constrained  mechanical  system.  Accordingly,  Eq.  (11.36)  is  some¬ 
times  called  first-order  kinematic  model.  In  view  of  the  results  recalled  in 
Appendix  D,  the  controllability  of  the  latter  guarantees  the  controllability  of 
system  (11.39). 

Summarizing,  in  nonholonomic  mechanical  systems  —  such  as  wheeled 
mobile  robots  —  it  is  possible  to  ‘cancel’  the  dynamic  effects  via  nonlinear 
state  feedback,  provided  that  the  dynamic  parameters  are  exactly  known  and 
the  complete  state  of  the  system  (generalized  coordinates  and  velocities  q  and 
q)  is  measured. 

Under  these  assumptions,  the  control  problem  can  be  directly  addressed  at 
the  (pseudo-)velocity  level,  i.e.,  by  choosing  v  in  such  a  way  that  the  kinematic 
model 

q  =  G(q)v 

behaves  as  desired.  From  v ,  it  is  possible  to  derive  the  actual  control  inputs 
at  the  generalized  force  level  through  (11.35).  Since  a  =  v  appears  in  this 
equation,  the  pseudo- velocities  v  must  be  differentiable  with  respect  to  time. 


Example  11.5 

For  illustration,  the  above  procedure  for  deriving,  reducing  and  partially  linearizing 
the  dynamic  model  is  now  applied  to  the  unicycle.  Let  m  be  the  mass  of  the  unicycle, 
/  its  moment  of  inertia  around  the  vertical  axis  through  its  centre,  ti  the  driving 
force  and  r 2  the  steering  torque.  With  the  kinematic  constraint  expressed  as  (11.12), 
the  dynamic  model  (11.28),  (11.29)  takes  on  the  form 


"  m  0  0" 

"aT 

0  m  0 
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O 

_#'_ 

"  cos  # 

O' 

r  -1 

sin  # 

sin  # 
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0 

1. 

Tl 

T2 

+ 

—cos  # 

0 

a:  sin#  —  y  cos#  =  0. 


In  this  case  one  has 


n{q,q)  =  0 
G(q)  =  S(q) 
GT(q)S(q)  =  I 
GT(q)BG(q)  =  0, 


and  thus  the  reduced  model  in  state-space  is  obtained  as 


q  =  G(q)v 
v  =  M~1(q)r 


1/m  0 

0  1/J 


where 


M~\q)  = 
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By  using  the  input  transformation 


r  =  Alu  = 


m 

0 


0 

I 


u, 


the  second-order  kinematic  model  is  obtained  as 


"  v  cos  9  - 

-o- 

-o- 
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0 
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10 

+ 

0 

Ml  + 
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0 
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.1. 

with  the  state  vector  £=[x  y  9  v  u)]T  £  IR5. 


11.5  Planning 

As  with  manipulators,  the  problem  of  planning  a  trajectory  for  a  mobile  robot 
can  be  broken  down  in  finding  a  path  and  defining  a  timing  law  on  the  path. 
However,  if  the  mobile  robot  is  subject  to  nonholonomic  constraints,  the  first 
of  these  two  subproblems  becomes  more  difficult  than  in  the  case  of  manipu¬ 
lators.  In  fact,  in  addition  to  meeting  the  boundary  conditions  (interpolation 
of  the  assigned  points  and  continuity  of  the  desired  degree)  the  path  must 
also  satisfy  the  nonholonomic  constraints  at  all  points. 


11.5.1  Path  and  Timing  Law 


Assume  that  one  wants  to  plan  a  trajectory  q(t),  for  t  €  that  leads  a 

mobile  robot  from  an  initial  configuration  q(ti)  =  qt  to  a  final  configuration 
q{tf)  =  qr  in  the  absence  of  obstacles.  The  trajectory  q(t)  can  be  broken 
down  into  a  geometric  path  q(s),  with  dq{s)/ds  ^  0  for  any  value  of  s,  and 
a  timing  law  s  =  s(t),  with  the  parameter  s  varying  between  s(ij)  =  Sj  and 
s(tf)  =  Sf  in  a  monotonic  fashion,  i.e.,  with  s(t)  >  0,  for  t  €  [U,tf].  A  possible 
choice  for  s  is  the  arc  length  along  the  path;  in  this  case,  it  would  be  Sj  =  0 
and  Sf  =  L,  where  L  is  the  length  of  the  path. 

The  above  space-time  separation  implies  that 


q  = 


dq 

dt 


=  q  s, 


where  the  prime  symbol  denotes  differentiation  with  respect  to  s.  The  gener¬ 
alized  velocity  vector  is  then  obtained  as  the  product  of  the  vector  q',  which 
is  directed  as  the  tangent  to  the  path  in  configuration  space,  by  the  scalar  s, 
that  varies  its  modulus.  Note  that  the  vector  [ x '  y']T  €  IR2  is  directed  as 
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the  tangent  to  the  Cartesian  path,  and  has  unit  norm  if  s  is  the  Cartesian  arc 
length  (see  Sect.  4.3.1). 

Nonholonomic  constraints  of  the  form  (11.3)  can  then  be  rewritten  as 

A(q)q  =  A(q)q's  =  0. 


If  s(t)  >  0,  for  t  €  [U,tf],  one  has 

A(q)q’  =  0.  (11.40) 

This  condition,  that  must  be  verified  at  all  points  by  the  tangent  vector  on  the 
configuration  space  path,  characterizes  the  notion  of  geometric  path  admissi¬ 
bility  induced  by  the  kinematic  constraint  (11.3)  that  actually  affects  gener¬ 
alized  velocities.  Similar  to  what  has  been  done  for  trajectories  in  Sect.  11.2, 
geometrically  admissible  paths  can  be  explicitly  defined  as  the  solutions  of 
the  nonlinear  system 

q’  =  G(q)u,  (11-41) 

where  it  is  a  vector  of  geometric  inputs  that  are  related  to  the  velocity  inputs 
u  by  the  relationship  u(t)  =  u(s)s(t).  Once  the  geometric  inputs  u(s)  are 
assigned  for  s  £  [s * ,  s/] ,  the  path  of  the  robot  in  configuration  space  is  uniquely 
determined.  The  choice  of  a  timing  law  s  =  s(t),  for  t  £  [U,tf],  will  then 
identify  a  particular  trajectory  along  this  path. 

For  example,  in  the  case  of  a  mobile  robot  with  unicycle-like  kinematics, 
the  pure  rolling  constraint  (11.6)  entails  the  following  condition  for  geometric 
admissibility  of  the  path: 

[  sin  9  —cos  9  0]  q’  =  x'  sin#  —  y'  cos  9  =  0, 

that  simply  expresses  the  fact  that  the  tangent  to  the  Cartesian  path  must  be 
aligned  with  the  robot  sagittal  axis.  As  a  consequence,  a  path  whose  tangent 
is  discontinuous  (e.g.,  a  broken  line)  is  not  admissible,  unless  the  unicycle  is 
allowed  to  stop  at  discontinuity  points  by  setting  s  =  0  for  the  time  necessary 
to  rotate  on  the  spot  so  as  to  align  with  the  new  tangent. 

Geometrically  admissible  paths  for  the  unicycle  are  the  solutions  of  the 
system 


x'  =  v  cos  9 
y'  =  v  sin  9 
0'  =  w, 


where  v,w  are  related  to  v,u>  by 


v(t)  =  v(s)s(t) 
Lo(t)  =  u(s)s(t). 


(11.42) 


(11.43) 

(11.44) 
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11.5.2  Flat  Outputs 


Many  kinematic  models  of  mobile  robots,  including  the  unicycle  and  the  bicy¬ 
cle,  exhibit  a  property  known  as  differential  flatness,  that  is  particularly  rel¬ 
evant  in  planning  problems.  A  nonlinear  dynamic  system  x  =  f(x)  +  G{x)u 
is  differentially  flat  if  there  exists  a  set  of  outputs  y,  called  flat  outputs,  such 
that  the  state  x  and  the  control  inputs  u  can  be  expressed  algebraically  as  a 
function  of  y  and  its  time  derivatives  up  to  a  certain  order: 

x  =  x{y,y,y,...,y(r)) 
u  =  u(y,y,y,...,y(r)). 

As  a  consequence,  once  an  output  trajectory  is  assigned  for  y,  the  associ¬ 
ated  trajectory  of  the  state  x  and  history  of  control  inputs  u  are  uniquely 
determined. 

In  the  unicycle  and  bicycle  cases,  the  Cartesian  coordinates  are  indeed 
flat  outputs.  In  the  following,  this  property  is  established  for  the  unicycle. 
This  can  be  done  with  reference  to  either  the  kinematic  model  (11.13)  or 
the  geometric  model  (11.42).  For  simplicity,  refer  to  the  latter.  Its  first  two 
equations  imply  that,  given  a  Cartesian  path  (x(s),y(s)),  the  associated  state 
trajectory  is  q(s )  =  [x(s)  y(s)  0(s)]T  where 

0(s)  =  Atan2  x'(s))  +  kit  k  =  0,1.  (11.45) 


The  two  possible  choices  for  k  account  for  the  fact  that  the  same  Cartesian 
path  may  be  followed  moving  forward  (fc  =  0)  or  backward  (k  =  1).  If  the 
initial  orientation  of  the  robot  is  assigned,  only  one  of  the  choices  for  k  is 
correct.  The  geometric  inputs  that  drive  the  robot  along  the  Cartesian  path 
are  easily  obtained  from  (11. 42), (11. 45)  as 


v(s)  =  ±  \J  (x'(s))2  +  (y'(s))2 

~<  i  “  X"(SW(S ) 

^s>  (a;'(s))2  +  (y'(a))  2 

These  equations  deserve  some  comments: 


(11.46) 

(11.47) 


•  The  choice  of  the  sign  of  u(s)  depends  on  the  type  of  motion  (forward  or 
backward) . 

•  If  x'(s )  =  y'(s)  =  0  for  some  s  €  [s,,s/],  one  has  v(s)  =  0.  This  happens, 
for  example,  in  correspondence  of  cusps  (motion  inversions)  in  the  Carte¬ 
sian  path.  In  these  points,  Eq.  (11.45)  does  not  define  the  orientation,  that 
can  however  be  derived  by  continuity,  i.e.,  as  the  limit  of  its  right-hand  side 
for  s  — >  s~ .  Similar  arguments  can  be  repeated  for  the  steering  velocity  ui 
given  by  (11.47). 

•  The  possibility  of  reconstructing  6  and  uj  is  lost  when  the  Cartesian  tra¬ 
jectory  degenerates  to  a  point,  because  in  this  case  it  is  x'(s )  =  y'(s )  =  0 
identically. 
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It  is  interesting  to  note  that  for  driftless  dynamic  systems  —  like  the 
kinematic  models  of  mobile  robots  —  differential  flatness  is  a  necessary  and 
sufficient  condition  for  transformability  in  the  chained  form  introduced  in 
Sect.  11.3.  In  particular,  it  is  easy  to  prove  that  the  flat  outputs  of  a  (2 ,  n) 
chained  form  are  z\  and  zn,  from  which  it  is  possible  to  compute  all  the  other 
state  variables  as  well  as  the  associated  control  inputs.  For  example,  in  the 
case  of  the  (2,3)  chained  form  (11.25)  it  is 


Z3  .  Z1Z3  -  ZiZ3 

Z2  =  —  V\  =  Zl  V2  =  - To - ■ 

Zi  z{ 

Note  that  z2  and  v2  can  be  actually  reconstructed  only  if  £i(t)  0,  for 

t  G  [U,tf]. 

11.5.3  Path  Planning 

Whenever  a  mobile  robot  admits  a  set  of  flat  outputs  y ,  these  may  be  exploited 
to  solve  planning  problems  efficiently.  In  fact,  one  may  use  any  interpolation 
scheme  to  plan  the  path  of  y  in  such  a  way  as  to  satisfy  the  appropriate 
boundary  conditions.  The  evolution  of  the  other  configuration  variables,  to¬ 
gether  with  the  associated  control  inputs,  can  then  be  computed  algebraically 
from  y(s).  The  resulting  configuration  space  path  will  automatically  satisfy 
the  nonholonomic  constraints  (11.40). 

In  particular,  consider  the  problem  of  planning  a  path  that  leads  a  unicycle 
from  an  initial  configuration  q(si)  =  =  [xi  y,  0i]T  to  a  final  configura¬ 

tion  q{sf)  =  qf  =  [xf  yf  df]T. 

Planning  via  Cartesian  polynomials 

As  mentioned  above,  the  problem  can  be  solved  by  interpolating  the  initial 
values  x-i ,  yt  and  the  final  values  Xf,yf  of  the  flat  outputs  x,  y.  Letting  s*  =  0 
and  Sf  =  1,  one  may  use  the  following  cubic  polynomials: 

x(s)  =  s3xj  —  (s  —  1  )3Xi  +  axs2(s  —  1)  +  /3xs(s  —  l)2 

y(s)  =  S3yf  -  {s  -  1  )3yi  +  Oiys2{s  -  1)  +  / 3ys(s  -  l)2, 

that  automatically  satisfy  the  boundary  conditions  on  x,  y.  The  orientation 
at  each  point  being  related  to  x',  y'  by  (11.45),  it  is  also  necessary  to  impose 
the  additional  boundary  conditions 

a/(0)  =  hi  cos  0i  x'(l)  =  kf  cos  Of 

y'(0)  =  hi  sin  0,  y'(l)  =  k/sinOf, 

where  ^  0,  fc/  ^  0  are  free  parameters  that  must  however  have  the  same 
sign.  This  condition  is  necessary  to  guarantee  that  the  unicycle  arrives  in  q j 
with  the  same  kind  of  motion  (forward  or  backward)  with  which  it  leaves  qr; 
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in  fact,  since  x(s)  and  y(s)  are  cubic  polynomials,  the  Cartesian  path  does 
not  contain  motion  inversions  in  general. 

For  example,  by  letting  ki  =  kf  =  k>0,  one  obtains 


k  cos  Of  —  3  Xf 

Px 

k  cos  0i  +  3xi 

OLy  _ 

k  sin  Of  —  3yf 

_Py  _ 

k  sin  0i  +  3yi 

The  choice  of  ki  and  kf  has  a  precise  influence  on  the  obtained  path.  In  fact, 
by  using  (11.46)  it  is  easy  to  verify  that 

rf(O)  =  ki  v(l)  =  kf. 

The  evolution  of  the  robot  orientation  along  the  path  and  the  associated 
geometric  inputs  can  then  be  computed  by  using  Eqs.  (11.45)  and  (11.46), 
(11.47),  respectively. 


Planning  via  the  chained  form 


Another  technique,  which  can  be  immediately  generalized  to  other  kinematic 
models  of  mobile  robots  (e.g.,  the  bicycle),  is  planning  the  path  in  the  chained 
form  coordinates  z.  To  this  end,  it  is  first  necessary  to  compute  the  initial 
and  final  values  zt  and  Zf  that  correspond  to  q{  and  q} ,  by  using  the  change 
of  coordinates  (11.23).  It  is  then  sufficient  to  interpolate  the  initial  and  final 
values  of  z\  and  z3  (the  flat  outputs)  with  the  appropriate  boundary  conditions 
on  the  remaining  variable  Z2  =  z3/ z[. 

Again,  it  is  possible  to  adopt  a  cubic  polynomial  to  solve  the  problem.  As 
an  alternative,  one  may  use  polynomials  of  different  degree  for  x  and  y  in  order 
to  reduce  the  number  of  unknown  coefficients  to  be  computed.  For  example, 
under  the  assumption  Z\f  ^  Z\j,  consider  the  following  interpolation  scheme: 

Zl(s)  =  Zijs  -  (s  -  l)zi,j 

Z3(s)  =  S3Z3j  -  (s  -  l)3Z3y  +  «3S2(s  -  1)  +  @3 s(s  -  l)2, 


with  s  €  [0, 1].  Note  that  4(s)  is  constant  and  equal  to  z\j  —  z\f  0.  The 
unknowns  ct3,  /33  must  be  determined  by  imposing  the  boundary  conditions 


on  z2 : 


4(Q) 

4(0) 


=  Z2i 


4(1) 

4(4 


Z2f, 


from  which 


Ot3  =  Z2j(ZiJ  -  Zi,i)  -  3 z3j 
P3  =  Z2,i(ziJ  -  Ziti)  +  3 Z3y. 


This  scheme  cannot  be  directly  applied  when  Z\^  =  Z\j,  i.e.,  when  9i  —  Of.  To 
handle  this  singular  case,  one  may  introduce  a  via  point  qv  =  [xv  yv  0V  }T 
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such  that  9V  4  9i ,  and  solve  the  original  planning  problem  using  two  consecu¬ 
tive  paths,  the  first  from  qi  to  qv  and  the  second  from  qv  to  qf.  Another  pos¬ 
sibility,  which  avoids  the  introduction  of  the  via  point,  is  to  let  Z\j  =  Z\^  +  2tt 
(i.e.,  to  replace  9f  with  9f  +  27t);  this  obviously  corresponds  to  the  same  final 
configuration  of  the  unicycle.  With  the  resulting  manoeuvre,  the  robot  will 
reach  its  destination  while  performing  a  complete  rotation  of  orientation  along 
the  path. 

Once  the  path  has  been  planned  for  the  chained  form,  the  path  q(s)  in 
the  original  coordinates  and  the  associated  geometric  inputs  w(s)  are  recon¬ 
structed  by  inverting  the  change  of  coordinates  (11.23)  and  of  inputs  (11.24), 
respectively. 

Planning  via  parameterized  inputs 

A  conceptually  different  approach  to  path  planning  consists  of  writing  the 
inputs  -  rather  than  the  path  —  in  parameterized  form,  and  computing  the 
value  of  the  parameters  so  as  to  drive  the  robot  from  qt  to  qj.  Again,  it  is 
convenient  to  work  on  the  chained  form,  whose  equations  are  easily  integrable 
in  closed  form  under  appropriate  inputs.  For  the  sake  of  generality,  refer  to 
the  (2,n)  chained  form  (11.20),  whose  geometric  version  is 

z'i  =  Vi 

4  =  ^2 

4  =  z2Vl 


z'n  =  4-lb- 

Let  the  geometric  input  be  chosen  as 

V!  =  sgn(Z\)  (11.48) 

v2  =  c0  +  Ci 8  +  .  .  .  +  C„_2Sn  2,  (11.49) 

with  A  =  zij  —  Ziti  and  s  £  [s,,  s/]  =  [0,  |Z\|j.  Parameters  Co, . . . ,  cn_ 2  must 
be  chosen  so  as  to  give  z(s f)  =  z / .  It  is  possible  to  verify  that  such  condition 
is  expressed  as  a  linear  system  of  equations 


D{A) 


co 

Cl 


=  d(zi,zf,A ) 


^n— 2 


(11.50) 


where  matrix  D(A)  is  invertible  if  A  4  0.  For  example,  in  the  case  of  the 
(2,  3)  chained  form,  one  obtains 

Z2J  -  Z2,i 
Z3J  -  Z3,i  -  Z2,iA 


D  = 


1^1 


A 

sgn(Z\)4-  4- 


d  = 


(11.51) 
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If  zij  =  z\j  a  singular  case  is  met  that  can  be  handled  as  before. 

Both  z(s)  and  v(s)  must  then  be  converted  to  q(s)  and  u(s )  using  the 
inverse  coordinate  and  input  transformations  that  apply  to  the  specific  case. 

The  above  method  does  not  make  explicit  use  of  the  flat  outputs,  but 
relies  on  the  closed-form  integrability  of  the  chained  form,  whose  existence, 
as  already  mentioned,  is  equivalent  to  differential  flatness.  Note  also  that  in 
this  planning  scheme,  as  in  the  previous  two,  parameter  s  does  not  represent 
the  arc  length  on  the  path. 

Other  classes  of  parameterized  inputs  that  can  be  used  in  place  of  (11.48), 
(11.49)  are  sinusoidal  and  piecewise  constant  functions. 

Numerical  results 

For  illustration,  some  numerical  results  of  the  planning  methods  so  far  de¬ 
scribed  are  now  presented.  The  considered  vehicle  is  a  unicycle  that  must 
perform  various  ‘parking’  manoeuvres. 

Two  typical  paths  produced  by  the  planner  that  uses  cubic  Cartesian  poly¬ 
nomials  are  shown  in  Fig.  11.5.  As  already  noticed,  the  unicycle  never  inverts 
its  motion,  that  is  forward  in  these  two  manoeuvres  because  k  =  5  >  0.  For 
k  <  0,  the  manoeuvres  would  have  been  performed  in  backward  motion  with 
different  paths. 

In  Fig.  11.6,  the  same  planner  is  used  to  solve  a  parallel  parking  problem, 
in  which  the  difference  between  the  initial  and  the  final  configuration  of  the 
unicycle  is  a  pure  displacement  in  the  direction  orthogonal  to  the  sagittal 
axis.  Note  how  the  path  changes  as  hi  =  kf  =  k  is  changed;  in  particular, 
an  increase  in  k  leads  to  elongated  ‘take-off’  (from  qi)  and  ‘landing’  (on  q f) 
phases. 

Figure  11.7  refers  to  the  case  in  which  and  differ  only  for  the  value 
of  9  (a  pure  reorientation);  the  unicycle  leaves  the  initial  position  and  follows 
a  path  that  leads  back  to  it  with  the  correct  orientation.  This  behaviour  is  to 
be  expected,  because  with  this  planner  a  Cartesian  path  of  nonzero  length  is 
needed  to  achieve  any  kind  of  reconfiguration. 

To  allow  a  comparison,  the  same  planning  problems  have  also  been  solved 
with  the  method  based  on  the  use  of  parameterized  inputs  in  conjunction  with 
the  chained  form. 

Figure  11.8  shows  the  path  obtained  with  this  planner  for  the  same  two 
parking  problems  of  Fig.  11.5.  While  in  the  first  case  the  obtained  manoeuvre 
is  similar  to  the  one  obtained  before,  in  the  second  the  path  contains  a  cusp, 
corresponding  to  a  motion  inversion.  In  fact,  in  view  of  its  nature,  this  planner 
can  only  generate  paths  along  which  the  robot  orientation  stays  between  its 
initial  value  9i  and  its  final  value  9f. 

In  Fig.  11.9,  two  different  solutions  produced  by  this  planner  are  reported 
for  the  parallel  parking  problem  of  Fig.  11.6.  The  singularity  due  to  9f=  9 f 
has  been  solved  in  two  different  ways:  by  adding  a  via  point  qv ,  and  redefining 
9f  as  9f  =  9i  +  2tt.  Note  that  in  the  latter  case  the  path  produced  by  the 
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Fig.  11.5.  Two  parking  manoeuvres  planned  via  cubic  Cartesian  polynomials;  in 
both  cases  k  =  5  has  been  used 
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Fig.  11.6.  Planning  a  parallel  parking  manoeuvre  via  cubic  Cartesian  polynomials; 
left:  with  k  =  10,  right:  with  k  =  20 
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Fig.  11.7.  Planning  a  pure  reorientation  manoeuvre  via  cubic  Cartesian  polynomi¬ 
als;  left:  with  k  =  10,  right:  with  k  =  20 


planner  leads  the  robot  to  the  destination  with  a  complete  rotation  of  its 
orientation. 

Finally,  the  same  pure  reorientation  manoeuvre  of  Fig.  11.7  has  been  con¬ 
sidered  in  Fig.  11.10.  The  path  on  the  left  has  been  obtained  as  outlined 
before,  i.e. ,  exploiting  the  transformations  of  coordinates  (11.23)  and  of  in- 
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Fig.  11.8.  Two  parking  manoeuvres  planned  via  the  chained  form 


[m] 


[m] 


Fig.  11.9.  Planning  a  parallel  parking  manoeuvre  via  the  chained  form;  left:  adding 
a  via  point  qv,  right:  letting  6f  =  6i  +  2n 


Fig.  11.10.  Planning  a  pure  reorientation  manoeuvre  via  the  chained  form;  left: 
with  the  coordinate  transformation  (11.23),  right:  with  the  coordinate  transforma¬ 
tion  (11.52) 


puts  (11.24)  to  put  the  system  in  chained  form,  and  then  using  the  parame¬ 
terized  inputs  (11.48),  (11.49).  As  in  the  previous  case,  the  required  reorien¬ 
tation  is  realized  by  a  Cartesian  path.  This  is  a  consequence  of  the  structure 
of  (11.23),  for  which  6^  7^  Of  implies  in  general  7^  22,/  and  23, i  7^  z$j, 
even  when  x*  =  Xf,  yi  =  yf. 
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The  manoeuvre  on  the  right  of  Fig.  11.10,  which  is  a  rotation  on  the  spot, 
was  achieved  by  using  a  different  change  of  coordinates  to  put  the  system  in 
(2,3)  chained  form.  In  particular,  the  following  transformation  has  been  used 

z\  =  9  —  Of 

Zi  =  (x  —  Xi )  cos  9  +  (y  —  yf)  sin  9  (11.52) 

Z3  =  (x  -  Xi)  sin  9  -  (y  -  yi)  cos  9 , 

which  places  the  origin  of  the  (22,  Z3)  reference  frame  in  correspondence  of  the 
initial  Cartesian  position  of  the  unicycle.  With  this  choice  one  has  22,?:  =  22,/ 
and  Z3f  =  Z3J  for  a  pure  reorientation,  and  thus  the  manoeuvre  is  efficiently 
obtained  as  a  simple  rotation. 

As  a  matter  of  fact,  using  (11.52)  in  place  of  (11.23)  is  always  recom¬ 
mended.  In  fact,  the  analysis  of  (11.51)  shows  that  in  general  the  magnitude 

of  the  coefficients  of  V2  —  and  therefore,  the  length  of  the  obtained  path 
depends  not  only  on  the  amount  of  reconfiguration  required  for  Z2  and  23,  but 
also  on  the  value  of  2:2,1  itself.  The  adoption  of  (11.52),  which  implies  22,,:  =  0, 
makes  the  size  of  the  manoeuvre  invariant  with  respect  to  the  Cartesian  po¬ 
sition  of  the  unicycle. 


11.5.4  Trajectory  Planning 

Once  a  path  q(s),  s  €  [sj,s/],  has  been  determined,  it  is  possible  to  choose 
a  timing  law  s  =  s(t )  with  which  the  robot  should  follow  it.  In  this  respect, 
considerations  similar  to  those  of  Sect.  4.3.1  apply.  For  example,  if  the  velocity 
inputs  of  the  unicycle  are  subject  to  bounds  of  the  form8 

|f(t)|  ^  Vmax  |w(t)|  5:  ^max  Vt,  (11.53) 


it  is  necessary  to  verify  whether  the  velocities  along  the  planned  trajectory 
are  admissible.  In  the  negative  case,  it  is  possible  to  slow  down  the  timing  law 
via  uniform  scaling.  To  this  end,  it  is  convenient  to  rewrite  the  timing  law 
by  replacing  t  with  the  normalized  time  variable  r  =  t/T ,  with  T  =  tf  —  ti- 
From  (11.43),  (11.44)  one  has 


v(t) 

w(<) 


v(s) 

w(s) 


ds  dr 
dr  dt 
ds  dr 
dr  dt 


.ds  1 
Via)irT 
.ds  1 


(11.54) 

(11.55) 


and  therefore  is  is  sufficient  to  increase  T  (i.e.,  the  duration  of  the  trajectory) 
to  reduce  uniformly  v  and  to,  so  as  to  stay  within  the  given  bounds. 


For  a  differential  drive  unicycle,  the  actual  bounds  affect  the  wheel  angular  speeds 
u>l  and  lor.  Through  Eqs.  (11.14),  these  bounds  can  be  mapped  to  constraints 
on  v  and  u  (see  Problem  11.9). 
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It  is  also  possible  to  plan  directly  a  trajectory  without  separating  the 
geometric  path  from  the  timing  law.  To  this  end,  all  the  techniques  presented 
before  can  be  used  with  the  time  variable  t  directly  in  place  of  the  path 
parameter  s.  A  drawback  of  this  approach  is  that  the  duration  tf  —  ti  =  Sf  —  Si 
of  the  trajectory  is  fixed,  and  uniform  scaling  cannot  be  used  to  satisfy  bounds 
on  the  velocity  inputs.  In  fact,  an  increase  (or  decrease)  of  tf—ti  would  modify 
the  geometric  path  associated  with  the  planned  trajectory. 

11.5.5  Optimal  Trajectories 

The  planning  techniques  so  far  presented  can  be  used  to  compute  trajectories 
that  lead  the  robot  from  an  initial  configuration  qi  to  a  final  configuration  q j 
while  complying  with  the  nonholonomic  constraints  and,  possibly,  bounds  on 
the  velocity  inputs.  Often,  other  requirements  are  added,  such  as  limiting  the 
path  curvature,  avoiding  workspace  obstacles  or  reducing  energy  consumption. 
In  general,  these  are  integrated  in  the  design  procedure  as  the  optimization 
of  a  suitable  cost  criterion  along  the  trajectory.  For  example,  the  previous 
objectives  will  be  respectively  formulated  as  the  minimization  of  the  maximum 
curvature,  the  maximization  of  the  minimum  distance  between  the  robot  and 
the  obstacles,  or  the  minimization  of  the  total  energy  needed  by  the  mobile 
robot  to  follow  the  path. 

A  simple  technique  for  attacking  the  optimal  planning  problem  consists 
of  over-parameterizing  the  adopted  interpolation  scheme,  so  as  to  pursue  the 
optimization  —  typically,  via  numerical  techniques  —  of  the  cost  criterion 
by  appropriately  choosing  the  redundant  parameters.  Clearly,  the  obtained 
trajectory  will  be  optimal  only  with  respect  to  the  set  of  trajectories  that 
can  be  generated  by  the  chosen  scheme,  and  will  be  a  suboptimal  solution  for 
the  original  planning  problem;  this  may  or  may  not  lead  to  the  fulfilment  of 
the  original  specifications.  For  example,  the  planning  scheme  based  on  cubic 
Cartesian  polynomials  contains  two  free  parameters  (kt  and  kp),  that  may 
be  chosen  so  as  to  maximize  the  minimum  distance  along  the  path  between 
the  unicycle  and  certain  obstacles.  However,  depending  on  the  placement  of 
the  obstacles  with  respect  to  q{  and  Q/,  a  collision-free  path  (i.e.,  a  path  for 
which  the  above  distance  is  always  positive)  may  or  may  not9  exist  within  the 
chosen  family  of  cubic  polynomials. 

A  more  systematic  approach  to  the  problem  relies  on  the  use  of  optimal 
control  theory.  The  basic  problem  considered  in  this  discipline  is  in  fact  the 
determination  of  a  control  law  that  transfers  a  dynamic  system  between  two 
assigned  states  so  as  to  minimize  a  chosen  cost  functional  along  the  trajectory. 
A  powerful  tool  for  solving  this  problem  is  the  Pontryagin  minimum  principle 
that  provides  necessary  conditions  for  optimality.  By  exploiting  these  condi¬ 
tions  in  conjunction  with  the  analysis  of  the  specific  characteristics  of  the 

9  The  complexity  of  the  problem  of  planning  collision- free  motions  in  the  presence  of 
obstacles  is  such  that  specific  solution  techniques  are  needed;  these  are  presented 
in  Chap.  12. 
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Fig.  11.11.  The  elementary  arcs  that  constitute  the  trajectories  of  the  sufficient 
family  for  the  minimum-time  planning  problem  for  the  unicycle 


considered  problem,  it  is  often  possible  to  identify  a  reduced  set  of  candi¬ 
date  trajectories,  also  referred  to  as  a  sufficient  family ,  among  which  there  is 
certainly  the  desired  optimal  solution  (if  it  exists). 

In  any  case,  each  optimal  planning  problem  must  be  formulated  in  the 
appropriate  context.  When  minimizing  curvature  or  avoiding  static  obstacles, 
the  timing  law  part  of  the  trajectory  is  irrelevant,  and  the  problem  can  be 
solved  by  planning  a  path  for  the  geometric  model  (11.41).  If  the  cost  criterion 
depends  on  the  path  as  well  as  on  the  timing  law,  it  is  necessary  to  plan 
directly  on  the  kinematic  model  (11.10).  A  particularly  important  example  of 
the  latter  situation  is  met  when  minimum-time  trajectories  are  sought  in  the 
presence  of  bounds  on  the  velocity  inputs. 

Minimum-time  trajectories 

Consider  the  problem  of  transferring  the  unicycle  (11.13)  from  the  initial 
configuration  qi  to  the  final  configuration  q ^  while  minimizing  the  functional 

ftf 

J  =  tf  —  ti  =  /  cit, 

under  the  assumption  that  the  driving  and  steering  velocity  inputs  v  and  u>  are 
bounded  as  in  (11.53).  By  combining  the  conditions  provided  by  the  minimum 
principle  with  geometrical  arguments,  it  is  possible  to  determine  a  sufficient 
family  for  the  solution  to  this  problem.  This  family  consists  of  trajectories 
obtained  by  concatenating  elementary  arcs  of  two  kinds  only: 

•  arcs  of  circle  of  variable  length,  covered  with  velocities  v(t)  =  ±r;max  and 
w(t)  =  ±wmax  (the  radius  of  the  circle  is  always  umax/o>max); 

•  line  segments  of  variable  length,  covered  with  velocities  v(t)  =  ±vmax  and 
u(t)  =  0. 

These  elementary  arcs  are  shown  in  Fig.  11.11,  where  a  compact  notation 
is  also  defined  for  identifying  them.  In  particular,  Ca  and  Sd  indicate  an  arc 
of  circle  of  duration  a  and  a  line  segment  of  duration  d,  respectively  (in  the 
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particular  case  umax  =  1,  a  and  d  are  also  the  lengths  of  these  arcs).  The 
superscript  indicates  forward  (+)  or  backward  (— )  motion,  while  for  circular 
arcs  the  second  subscript  indicates  a  rotation  in  the  clockwise  (r)  or  coun¬ 
terclockwise  (l)  direction.  With  this  notation,  and  considering  for  simplicity 
the  case  umax  =  1  and  wmax  =  1,  the  trajectories  of  the  sufficient  family  (also 
called  Reeds-Shepp  curves )  can  be  classified  in  the  following  nine  groups: 


I 

ca\cb\ce 

a>0,b>0,e>0,a  +  b  +  e<n 

II 

ca\cbce 

0  <  a  <  b,  0  <  e  <  b,  0  <  b  <  7r/2 

III 

cacb\ce 

0<a<b,0<e<b,  0  <  b  <  7r/2 

IV 

Cacb\cbce 

0  <  a  <  b,  0  <  e  <  b,  0  <  b  <  7r/2 

V 

ca\cbcb\ce 

0  <  a  <  6, 0  <  e  <  6, 0  <  b  <  w/2 

VI 

Ca\Cv/2SeCv/2\Cb 

0  <  a  <  7r/2, 0  <  b  <  tt/2,  e  >  0 

VII 

ca\cn/2secb 

0<a<7r,0<&<  7r/2,  e  >  0 

VIII 

casec„/2\cb 

0  <  a  <  tt/2,  0  <  b  <  tt,  e  >0 

IX 

casecb 

0<a<7r/2,  0<6<7r/2,e>0, 

where  the  symbol  “|”  between  two  elementary  arcs  indicates  the  presence 
of  a  cusp  (motion  inversion)  on  the  path.  Each  group  contains  trajectories 
consisting  of  a  sequence  of  no  more  than  five  elementary  arcs;  the  duration  of 
circular  arcs  is  bounded  by  either  7r/2  or  7r,  while  the  duration  of  line  segments 
depends  on  the  Cartesian  distance  between  qi  and  q j .  The  number  of  possible 
sequences  produced  by  each  group  is  finite;  they  are  obtained  by  instantiating 
the  elementary  arcs  depending  on  the  direction  of  motion  and  of  rotation.  For 
example,  it  is  easy  to  show  that  group  IX  generates  eight  sequence  types,  each 
corresponding  to  a  trajectory  entirely  covered  in  forward  or  backward  motion: 


ri+  q+/°H-  /^+  /^+  c+/^+  ri+  C+ /'■''+ 

° a,r°e  W,r’  °a,r°e  ua,l’  °a,Z°e  ua,r)  L'a,Z 

c-rs~c-r,  c-rs-c-u  c-fi-c-,,  c-.s-c-j. 


By  this  kind  of  argument,  it  may  be  verified  that  the  above  groups  generate 
a  total  number  of  48  different  sequences. 

In  practice,  one  may  use  an  exhaustive  algorithm  to  identify  the  minimum¬ 
time  trajectory  that  leads  the  unicycle  from  q{  to  qj\ 

•  Determine  all  the  trajectories  belonging  to  the  sufficient  family  that  join 
q,  and  qf. 

•  Compute  the  value  of  the  cost  criterion  tf  —  t,  along  these  trajectories, 
and  choose  the  one  to  which  the  minimum  value  is  associated. 


The  first  is  clearly  the  most  difficult  step.  Essentially,  for  each  of  the  afore¬ 
mentioned  48  sequences  it  is  necessary  to  identify  —  if  it  exists  —  the  cor¬ 
responding  trajectory  going  from  qt  to  q^,  and  to  compute  the  duration  of 
the  associated  elementary  arcs.  To  this  end,  for  each  sequence,  it  is  possibly 
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Fig.  11.12.  Two  examples  of  minimum-time  trajectories  for  the  unicycle 


to  express  and  invert  in  closed  form  the  relationship  between  the  duration  of 
the  arcs  and  the  obtained  change  in  configuration.  By  doing  so,  the  first  step 
can  also  be  completed  very  quickly. 

Figure  11.12  shows  two  examples  of  minimum-time  trajectories  for  the 
unicycle.  The  first  (on  the  left)  belongs  to  group  IX  and  contains  three  el¬ 
ementary  arcs  without  inversions  of  motion.  The  second  (on  the  right)  is  a 
group  V  trajectory  consisting  of  four  arcs  of  circle,  along  which  there  are  two 
motion  inversions. 


11.6  Motion  Control 

The  motion  control  problem  for  wheeled  mobile  robots  is  generally  formulated 
with  reference  to  the  kinematic  model  (11.10),  i.e. ,  by  assuming  that  the 
control  inputs  determine  directly  the  generalized  velocities  q.  For  example,  in 
the  case  of  the  unicycle  (11.13)  and  of  the  bicycle  (11.18)  or  (11.19)  this  means 
that  the  inputs  are  the  driving  and  steering  velocity  inputs  v  and  ui.  There 
are  essentially  two  reasons  for  taking  this  simplifying  assumption.  First,  as 
already  seen  in  Sect.  11.4,  under  suitable  assumptions  it  is  possible  to  cancel 
the  dynamic  effects  via  state  feedback,  so  that  the  control  problem  is  actually 
transferred  to  the  second-order  kinematic  model,  and  from  the  latter  to  the 
first-order  kinematic  model.  Second,  in  the  majority  of  mobile  robots  it  is  not 
possible  to  command  directly  the  wheel  torques,  because  there  are  low-level 
control  loops  that  are  integrated  in  the  hardware  or  software  architecture. 
These  loops  accept  as  input  a  reference  value  for  the  wheel  angular  speed, 
that  is  reproduced  as  accurately  as  possible  by  standard  regulation  actions 
(e.g.,  PID  controllers).  In  this  situation,  the  actual  inputs  available  for  high- 
level  controls  are  precisely  the  reference  velocities. 

In  this  section,  a  unicycle-like  vehicle  is  again  considered,  although  some 
of  the  presented  control  schemes  may  be  extended  to  other  kinds  of  mobile 
robots.  Two  basic  control  problems,  illustrated  in  Fig.  11.13,  will  be  considered 
for  system  (11.13): 
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Fig.  11.13.  Control  problems  for  a  unicycle;  left:  trajectory  tracking,  right:  posture 
regulation 


•  Trajectory  tracking:  the  robot  must  asymptotically  track  a  desired  Carte¬ 
sian  trajectory  ( Xd(t),yd(t )),  starting  from  an  initial  configuration  q0  = 
[xo  yo  Qo]T  that  may  or  may  not  be  ‘matched’  with  the  trajectory. 

•  Posture  regulation:  the  robot  must  asymptotically  reach  a  given  posture, 
i.e.,  a  desired  configuration  qd,  starting  from  an  initial  configuration  q0. 

From  a  practical  point  of  view,  the  most  relevant  of  these  problems  is 
certainly  the  first.  This  is  because,  unlike  industrial  manipulators,  mobile 
robots  must  invariably  operate  in  unstructured  workspaces  containing  ob¬ 
stacles.  Clearly,  forcing  the  robot  to  move  along  (or  close  to)  a  trajectory 
planned  in  advance  considerably  reduces  the  risk  of  collisions.  On  the  other 
hand,  a  preliminary  planning  step  is  not  required  when  posture  regulation  is 
performed,  but  the  Cartesian  trajectory  along  which  the  robot  approaches  qd 
cannot  be  specified  by  the  user. 

11.6.1  Trajectory  Tracking 

For  the  tracking  problem  to  be  soluble,  it  is  necessary  that  the  desired  Carte¬ 
sian  trajectory  (xd(t),yd(t))  is  admissible  for  the  kinematic  model  (11.13), 
i.e.,  it  must  satisfy  the  equations 

id  =  vd  cos  Qd 

Vd  =  Vd  sin  9d  (11.57) 

Qd  =  Ud 

for  some  choice  of  the  reference  inputs  Vd  and  u>d-  For  example,  this  is  certainly 
true  if  the  trajectory  has  been  produced  using  one  of  the  planning  schemes  of 
the  previous  section.  In  any  case,  as  seen  in  Sect.  11.5.2,  since  the  unicycle  co¬ 
ordinates  x  and  y  are  flat  outputs,  the  orientation  along  the  desired  trajectory 
(xd(t),yd{t))  can  be  computed  as 


Qd{t)  =  Atan2  {yd{t),  xd(t))  +  kn  k  =  0, 1, 


(11.58) 
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as  well  as  the  reference  inputs 


Vd(t)  =  ±\/i:d(t)+yd(t) 

_  yd(t)xd(t )  -  xd(t)yd(t) 

^dV-)  —  -2U\  i  „\2/-A 


(11.59) 

(11.60) 


Note  that  (11.58)  and  (11.59),  (11.60),  correspond  respectively  to  (11.45) 
and  (11.46),  (11.47)  with  s  =  t.  In  the  following,  it  is  assumed  that  the  value 
of  k  in  (11.58)  —  and  correspondingly,  the  sign  of  vd  in  (11.59)  —  has  been 
chosen. 

By  comparing  the  desired  state  qd(t)  =  [xd(t)  yd(t)  9d(t)]T  with  the 
current  measured  state  q(t)  =  [x(t)  y(t)  9(t))T  it  is  possible  to  compute 
an  error  vector  that  can  be  fed  to  the  controller.  However,  rather  than  using 
directly  the  difference  between  qd  and  q,  it  is  convenient  to  define  the  tracking 
error  as 


ei 

cos  9  sin  9  0 

1 

e  = 

e-2 

= 

—sin  9  cos  9  0 

yd-y 

e3 

0  0  1 

ed-9 

The  positional  part  of  e  is  the  Cartesian  error  ep  =  [xd  —  x  yd  —  y]T  ex¬ 
pressed  in  a  reference  frame  aligned  with  the  current  orientation  9  of  the  robot 
(see  Fig.  11.13).  By  differentiating  e  with  respect  to  time,  and  using  (11.13) 
and  (11.57),  one  easily  finds 


ei  =  vd  cos  —  v  +  e2U> 

6-2  =  'Xd  sin  €-z  —  C\U)  (11.61) 

e3  =  X)d  -  LO. 

Using  the  input  transformation 

v=vdcosez  —  ui  (11.62) 

to  =  ud  -  i/2,  (11.63) 


which  is  clearly  invertible,  the  following  expression  is  obtained  for  the  tracking 
error  dynamics: 


0 

OJd 

o' 

0 

e  = 

~x>d 

0 

0 

e  + 

sin  e3 

Vd  + 

0 

0 

0 

0 

u  i 
U2 


(11.64) 


Note  that  the  first  term  of  this  dynamics  is  linear,  while  the  second  and  third 
are  nonlinear.  Moreover,  the  first  and  second  terms  are  in  general  time- varying, 
due  to  the  presence  of  the  reference  inputs  vd(t)  and  tod(t). 
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Control  based  on  approximate  linearization 

The  simplest  approach  to  designing  a  tracking  controller  consists  of  using  the 
approximate  linearization  of  the  error  dynamics  around  the  reference  trajec¬ 
tory,  on  which  clearly  e  =  0.  This  approximation,  whose  accuracy  increases 
as  the  tracking  error  e  decreases,  is  obtained  from  (11.64)  simply  setting 
sin  e3  =  e3  and  evaluating  the  input  matrix  on  the  trajectory.  The  result  is 

'0  ud  0  1  [l.  O' 

e  =  —  aid  0  Vd  e  +  0  0 

o  o  oj  [°  i 

Note  that  the  approximate  system  is  still  time-varying.  Consider  now  the 
linear  feedback 

Mi  =  —  ki  ei  (11.66) 

m2  =  —  fc2  e2  —  k3  e3  (11.67) 

that  leads  to  the  following  closed-loop  linearized  dynamics: 

-ki  iod  0 

e  =  A(t)e=  —u>d  0  vd  e.  (11.68) 

0  — fc2  — fc3 

The  characteristic  polynomial  of  matrix  A  is 

p{  A)  =  A(A  +  fci)(A  +  ks)  +  u)d(  A  +  k%)  +  Vdk2{  A  +  fci). 

At  this  point,  it  is  sufficient  to  let 

2  2 

ki  =  /c3  =  2C,a  k2  =  — - — ,  (11.69) 

Vd 

with  (  G  (0, 1)  and  a  >  0,  to  obtain 

p(  A)  =  (A  +  2(a)(A2  +  2(aA  +  a2). 

The  closed-loop  linearized  error  dynamics  is  then  characterized  by  three  con¬ 
stant  eigenvalues:  one  real  negative  eigenvalue  in  —2 (a  and  a  pair  of  complex 
eigenvalues  with  negative  real  part,  damping  coefficient  (  and  natural  fre¬ 
quency  a.  However,  in  view  of  its  time-varying  nature,  there  is  no  guarantee 
that  system  (11.68)  is  asymptotically  stable. 

A  notable  exception  is  when  vd  and  u>d  are  constant,  as  in  the  case  of  cir¬ 
cular  or  rectilinear  trajectories.  In  fact,  the  linearized  system  (11.68)  is  then 
time-invariant  and  therefore  asymptotically  stable  with  the  choice  of  gains 
in  (11.69).  Hence,  by  using  the  control  law  (11.66),  (11.67)  with  the  same 
gains,  the  origin  of  the  original  error  system  (11.64)  is  also  asymptotically 
stable,  although  this  result  is  not  guaranteed  to  hold  globally.  For  sufficiently 
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small  initial  errors,  the  unicycle  will  certainly  converge  to  the  desired  Carte¬ 
sian  trajectory  (either  circular  or  rectilinear). 

In  general,  the  feedback  controller  (11.66),  (11.67)  is  linear  but  also  time- 
varying  in  view  of  the  expression  of  fe  in  (11.69).  The  actual  velocity  inputs 
v  and  u>  should  be  reconstructed  from  U\  and  u2  through  (11.62),  (11.63). 
In  particular,  it  is  easy  to  verify  that  v  and  u>  tend  to  coincide  with  the 
reference  inputs  Vd  and  u>d  (i.e.,  they  reduce  to  a  pure  feedforward  action)  as 
the  tracking  error  e  vanishes. 

Finally,  note  that  fe  in  (11.69)  diverges  when  Vd  goes  to  zero,  i.e.,  when 
the  reference  Cartesian  trajectory  tends  to  stop.  Therefore,  the  above  control 
scheme  can  only  be  used  for  persistent  Cartesian  trajectories,  i.e.,  trajectories 
such  that  |c,i(t)|  >  v  >  0,V£  >  0.  This  also  means  that  motion  inversions 
(from  forward  to  backward  motion,  or  vice  versa)  on  the  reference  trajectory 
are  not  allowed. 

Nonlinear  control 

Consider  again  the  exact  expression  (11.64)  of  the  tracking  error  dynamics, 
now  rewritten  for  convenience  in  the  ‘mixed’  form: 

ei  =  e2  u>  +  u\ 

fe  =  Vd  sin  e3  —  e^ui  (11.70) 

e3  =  u2, 

and  the  following  nonlinear  version  of  the  control  law  (11.66),  (11.67): 

ui  = -ki(vd,uJd)ei  (11.71) 

u2  =  -fe  Vd  Sin-e^  e2  -  k3(vd,u>d )  e3,  (11.72) 

e3 

where  fe(-,  •)  >  0  and  fe(-,  •)  >  0  are  bounded  functions  with  bounded  deriva¬ 
tives,  and  fe  >  0  is  constant.  If  the  reference  inputs  Vd  and  Ud  are  also  bounded 
with  bounded  derivatives,  and  they  do  not  both  converge  to  zero,  the  tracking 
error  e  converges  to  zero  globally,  i.e.,  for  any  initial  condition. 

A  sketch  is  now  given  of  the  proof  of  this  result.  Consider  the  closed-loop 
error  dynamics 


ei  =  e2  u  -  ki(vd,u>d )  ei 
fe  =  Vd  sin  e3  —  e±  u 

e3  =  -fe  fdSme3  e2  -  k3(vd,cJd)  e3, 
e3 

and  the  candidate  Lyapunov  function 


(11.73) 
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whose  time  derivative  along  the  system  trajectories 

V  =  -ki(vd,u)d)k 2  e\  -  k3(vd,Ud)el 

is  negative  semi-definite.  This  means  that  V  (which  is  bounded  below)  tends 
to  a  limit  value,  and  also  that  the  norm  of  e  is  bounded.  As  system  (11.73)  is 
time- varying,  it  is  not  possible  to  use  La  Salle  theorem  to  gain  further  insight. 
However,  under  the  above  assumptions,  one  may  verify  that  V  is  limited,  and 
therefore  V  is  uniformly  continuous.  Hence,  Barbalat  lemma  (see  Sect.  C.3) 
may  be  used  to  conclude  that  V  tends  to  zero,  i.e. ,  that  ei  and  e 3  tend  to 
zero.  From  this  and  the  system  equations  it  is  possible  to  prove  that 

lim  (v2d  +  u2d)e22  =  0, 

t—>  OO 

and  thus  e2  tends  to  zero  as  well,  provided  that  at  least  one  of  the  reference 
inputs  is  persistent. 

Again,  the  actual  velocity  inputs  v  and  u>  must  be  computed  from  U\  and 
u2  using  (11.62),  (11.63).  Note  that  the  control  law  (11.71),  (11.72)  requires 
the  persistency  of  the  state  trajectory  q(t),  but  not  of  the  Cartesian  trajectory. 
In  particular,  the  reference  velocity  vd(t)  can  converge  to  zero  as  long  as  ujd(t) 
does  not,  and  vice  versa.  For  example,  this  controller  can  be  used  to  track  a 
Cartesian  trajectory  that  degenerates  to  a  simple  rotation  on  the  spot. 


Input/output  linearization 

A  well-known  systematic  approach  to  the  design  of  trajectory  tracking  con¬ 
trollers  is  based  on  input/output  linearization  via  feedback  (see  Sect.  8.5.2). 
In  the  case  of  the  unicycle,  consider  the  following  outputs: 


■yi  =  x  +  b  cos  8 
V2  =  y  +  b  sin0, 


with  b  ytz  0.  They  represent  the  Cartesian  coordinates  of  a  point  B  located 
along  the  sagittal  axis  of  the  unicycle  at  a  distance  \b\  from  the  contact  point 
of  the  wheel  with  the  ground  (see  Fig.  11.3).  In  particular,  B  is  ‘ahead’  of  the 
contact  point  if  b  is  positive  and  ‘behind’  if  it  is  negative. 

The  time  derivatives  of  y\  and  y2  are 


y  i 

cos  6 

—b  sin  8 

V 

sin  8 

b  cos  8 

i 0 

T{8) 


(11.74) 


Matrix  T(9)  has  determinant  6,  and  is  therefore  invertible  under  the  assump¬ 
tion  that  b  ^  0.  It  is  then  sufficient  to  use  the  following  input  transformation 


v 

<jj 


T~\9) 


u  1 
u2 


cos  8  sin  8 

Ul 

—  sin  8/b  cos  8/b 

_ u 2_ 
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cartesian  error 


Fig.  11.14.  Tracking  a  circular  reference  trajectory  ( dotted )  with  the  controller 
based  on  approximate  linearization;  left:  Cartesian  motion  of  the  unicycle,  right : 
time  evolution  of  the  norm  of  the  Cartesian  error  ep 


to  put  the  equations  of  the  unicycle  in  the  form 


m2  (11.75) 

u2  cos  6  —  u\  sin  6 

b  ' 

An  input/output  linearization  via  feedback  has  therefore  been  obtained.  At 
this  point,  a  simple  linear  controller  of  the  form 

«i  =  Vid  +  hiVid  ~  2/i )  (11.76) 

M2  =  V2d  +  k2(y2d  -  y2),  (11.77) 

with  k\  >  0,  k2  >  0,  guarantees  exponential  convergence  to  zero  of  the  Carte¬ 
sian  tracking  error,  with  decoupled  dynamics  on  its  two  components.  Note  that 
the  orientation,  whose  evolution  is  governed  by  the  third  equation  in  (11.75), 
is  not  controlled.  In  fact,  this  tracking  scheme  does  not  use  the  orientation 
error;  hence,  it  is  based  on  the  output  error  rather  than  the  state  error. 

It  should  be  emphasized  that  the  reference  Cartesian  trajectory  for  point 
B  can  be  arbitrary,  and  in  particular  the  associated  path  may  exhibit  isolated 
points  with  discontinuous  geometric  tangent  (like  in  a  broken  line)  without 
requiring  the  robot  to  stop  and  reorient  itself  at  those  points.  This  is  true  as 
long  as  b  ^  0,  and  therefore  such  a  possibility  does  not  apply  to  the  contact 
point  between  the  wheel  and  the  ground,  whose  velocity,  as  already  discussed, 
cannot  have  a  component  in  the  direction  orthogonal  to  the  sagittal  axis  of 
the  vehicle. 

Simulations 

An  example  of  application  of  the  trajectory  tracking  controller  (11.66),  (11.67) 
based  on  linear  approximation  is  shown  in  Fig.  11.14.  The  desired  trajectory 


V  i  = 

m  = 
e  = 
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cartesian  error 


Fig.  11.15.  Tracking  a  figure  of  eight-shaped  reference  trajectory  ( dotted )  with  the 
nonlinear  controller;  left:  Cartesian  motion  of  the  unicycle,  right :  time  evolution  of 
the  norm  of  the  Cartesian  error  ev 


in  this  case  is  the  circle  of  centre  ( xc,  yc )  and  radius  R  described  by  the 
parametric  equations 


xd{t)  =  xc  +  R  cos  (Ludt) 

Ud{t)  =  Dc  +  R  sin  (udt) , 

with  R  =  3  m  and  u>d  =  1/3  rad/s.  Hence,  the  reference  driving  velocity  on 
the  circle  is  constant  and  equal  to  Vd  =  Rx>d  =  1  m/s.  The  controller  gains 
have  been  chosen  according  to  (11.69)  with  (  =  0.7  and  a  =  1.  Note  the 
exponential  convergence  to  zero  of  the  Cartesian  error. 

In  the  second  simulation  (Fig.  11.15)  the  reference  trajectory  is  figure 
of  eight-shaped,  with  centre  in  (xc,yc),  and  is  described  by  the  parametric 
equations 


xd{t)  =  xc  +  Ri  sin  (2 udt) 

Vd(t)  =  yc  +  R2  sin  (i ujdt ), 

with  R\  =  1?2  =  3  m  and  u>d  —  1/15  rad/s.  The  reference  driving  velocity  Vd{t ) 
in  this  case  varies  over  time  and  must  be  computed  using  (11.59).  The  results 
shown  have  been  obtained  with  the  nonlinear  controller  (11.71),  (11.72),  with 
k\  and  k%  again  given  by  (11.69)  in  which  £  =  0.7  and  a  =  1,  while  has 
been  set  to  1.  Also  in  this  case,  the  error  converges  to  zero  very  quickly. 

The  third  Cartesian  reference  trajectory  is  a  square  with  a  side  of  4  m,  to 
be  traced  with  constant  velocity.  This  requirement  means  that  the  unicycle 
cannot  stop  at  the  vertices  in  order  to  reorient  itself,  and  therefore  the  rep¬ 
resentative  point  ( x ,  y)  of  the  unicycle  cannot  follow  the  reference  trajectory. 
As  a  consequence,  the  tracking  scheme  based  on  input /output  linearization 
has  been  adopted.  In  particular,  two  simulations  are  presented,  both  obtained 
using  the  control  law  (11.76),  (11.77)  with  fci  =  &2  =  2.  In  the  first,  the  point 
B  that  tracks  the  reference  trajectory  is  located  at  a  distance  b  =  0.75  m  from 
the  contact  point  between  the  wheel  and  the  ground.  As  shown  in  Fig.  11.16, 
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driving  velocity 


Fig.  11.16.  Tracking  a  square  reference  trajectory  ( dotted )  with  the  controller  based 
on  input/output  linearization,  with  b  =  0.75;  left:  Cartesian  motion  of  the  unicycle, 
right:  time  evolution  of  the  velocity  inputs  v  and  ui 
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Fig.  11.17.  Tracking  a  square  reference  trajectory  ( dotted )  with  the  controller  based 
on  input/output  linearization,  with  b  =  0.2;  left:  Cartesian  motion  of  the  unicycle, 
right:  time  evolution  of  the  velocity  inputs  v  and  io 


the  unicycle  actually  moves  on  a  ‘smoothed’  trajectory;  therefore,  an  obstacle- 
free  channel  around  the  trajectory  must  be  available  to  account  for  the  area 
swept  by  the  unicycle  in  correspondence  of  the  square  vertices. 

The  second  simulation  (Fig.  11.17)  shows  what  can  happen  when  b  is 
reduced  in  order  to  achieve  more  accurate  tracking  for  the  unicycle  represen¬ 
tative  point;  in  particular,  b  =  0.2  m  was  chosen  in  this  case.  While  it  is  true 
that  the  unicycle  tracks  the  square  more  closely  with  respect  to  the  first  sim¬ 
ulation,  the  steering  velocity  is  much  higher  in  correspondence  of  the  vertices, 
and  this  might  be  a  problem  in  the  presence  of  saturations  on  the  velocity 
inputs.  This  situation  is  consistent  with  the  fact  that  matrix  T  in  (11.74) 
tends  to  become  singular  when  b  approaches  zero. 


11.6.2  Regulation 

Consider  now  the  problem  of  designing  a  feedback  control  law  that  drives  the 
unicycle  (11.13)  to  a  desired  configuration  qd.  A  reasonable  approach  could 


11.6  Motion  Control  511 


be  to  plan  first  a  trajectory  that  stops  in  qd,  and  then  track  it  via  feedback. 
However,  none  of  the  tracking  schemes  so  far  described  can  be  used  to  this  end. 
In  fact,  the  controller  based  on  approximate  linearization  and  the  nonlinear 
controller  both  require  a  persistent  state  trajectory.  The  scheme  based  on 
input/output  linearization  can  also  track  non-persistent  trajectories,  but  will 
drive  point  B  to  the  destination  rather  than  the  representative  point  of  the 
unicycle.  Besides,  the  final  orientation  at  the  destination  will  not  be  controlled. 

Actually,  the  difficulty  of  identifying  feedback  control  laws  for  tracking 
non-persistent  trajectories  is  structural.  It  is  in  fact  possible  to  prove  that,  due 
to  the  nonholonomy  of  the  system,  the  unicycle  does  not  admit  any  universal 
controller,  i.e.,  a  controller  that  can  asymptotically  stabilize  arbitrary  state 
trajectories,  either  persistent  or  not.  This  situation  is  completely  different  from 
the  case  of  manipulators,  for  which  the  scheme  based  on  inverse  dynamics  is 
an  example  of  universal  controller.  As  a  consequence,  the  posture  regulation 
problem  in  nonholonomic  mobile  robots  must  be  addressed  using  purposely 
designed  control  laws. 


Cartesian  regulation 

Consider  first  the  problem  of  designing  a  feedback  controller  for  a  partial  regu¬ 
lation  task,  in  which  the  objective  is  to  drive  the  unicycle  to  a  given  Cartesian 
position,  without  specifying  the  final  orientation.  This  simplified  version  of  the 
regulation  problem  is  of  practical  interest.  For  example,  a  mobile  robot  ex¬ 
ploring  an  unknown  environment  must  visit  a  sequence  of  Cartesian  positions 
( view  points)  from  where  it  perceives  the  characteristics  of  the  surrounding 
area  using  its  on-board  sensors.  If  these  are  isotropically  distributed  on  the 
robot  (as  in  the  case  of  a  ring  of  equispaced  ultrasonic  sensors,  a  rotating  laser 
range  finder,  or  a  panoramic  camera),  the  orientation  of  the  robot  at  the  view 
point  is  irrelevant. 

Without  loss  of  generality,  assume  that  the  desired  Cartesian  position  is 
the  origin;  the  Cartesian  error  ep  is  then  simply  [—a;  —  y]T ■  Consider  the 

following  control  law 


v  =  —  ki(xcos9  +  ysinO)  (11.78) 

u>  =  fc2(Atan2(t/,  x)  —  9  +  7r),  (11.79) 

where  k\  >  0,  &2  >  0.  These  two  commands  have  an  immediate  geometric 
interpretation:  the  driving  velocity  v  is  proportional  to  the  projection  of  the 
Cartesian  error  ep  on  the  sagittal  axis  of  the  unicycle,  whereas  the  steering 
velocity  u>  is  proportional  to  the  difference  between  the  orientation  of  the 
unicycle  and  that  of  vector  ep  ( pointing  error). 

Consider  the  following  ‘Lyapunov-like’  function: 

V=\{x2+y% 
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that  is  only  positive  semi-definite  at  the  origin,  because  it  is  zero  in  all  configu¬ 
rations  such  that  x  =  y  =  0,  independently  from  the  value  of  the  orientation  9. 
Using  the  unicycle  equations  in  (11.13)  and  the  control  inputs  (11.78),  (11.79) 
one  obtains 

V  =  —ki(xcos9  +  ysmd)2, 

that  is  negative  semi-definite  at  the  origin.  This  indicates  that  V,  which  is 
bounded  below,  tends  to  a  limit  value,  and  also  that  the  position  error  ep  is 
bounded  in  norm.  It  is  easy  to  verify  that  V  is  also  bounded,  and  thus  V  is 
uniformly  continuous.  Barbalat  lemma10  implies  that  V  tends  to  zero.  Hence 

lim  (x  cos  9  +  y  sin  9)  =  0, 

t — >oo 

i.e.,  the  projection  of  the  Cartesian  error  vector  ep  on  the  sagittal  axis  of  the 
unicycle  tends  to  vanish.  This  cannot  happen  in  a  point  different  from  the 
origin,  because  the  steering  velocity  (11.79)  would  then  force  the  unicycle  to 
rotate  so  as  to  align  with  ep.  One  may  then  conclude  that  the  Cartesian  error 
tends  to  zero  for  any  initial  configuration. 


Posture  regulation 

To  design  a  feedback  controller  that  is  able  to  regulate  the  whole  configu¬ 
ration  vector  (Cartesian  position  and  vehicle  orientation)  of  the  unicycle,  it 
is  convenient  to  formulate  the  problem  in  polar  coordinates.  It  is  again  as¬ 
sumed,  without  loss  of  generality,  that  the  desired  configuration  is  the  origin 

Qd  =  [ 0  0  OF. 

With  reference  to  Fig.  11.18,  let  p  be  the  distance  between  the  represen¬ 
tative  point  (x,  y)  of  the  unicycle  and  the  origin  of  the  Cartesian  plane,  7  the 
angle  between  vector  ep  and  the  sagittal  axis  of  the  vehicle,  and  S  the  angle 
between  the  same  vector  and  the  x  axis.  In  formulae: 


p  =  v  x2  +  y2 
7  =  Atan2(y,  x)  —  9  +  n 
<5  =  7  +  0. 


In  these  coordinates,  the  kinematic  model  of  the  unicycle  is  expressed  as 


p  =  — tJeosy 
sin  7 

7  =  - V  —  UJ 

p 

i  sin  7 

0  =  - v. 

P 


(11.80) 


Note  that  the  input  vector  field  associated  with  v  is  singular  for  p  =  0. 

10  La  Salle  theorem  cannot  be  used  because  V  is  not  positive  definite  at  the  origin. 
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Fig.  11.18.  Definition  of  polar  coordinates  for  the  unicycle 


Define  the  feedback  control  as11 

v  =  kipcosj  (11.81) 

w  =  fe7+jfel5E2£^(7 +  *>*),  (11.82) 

7 

with  ki  >  0,  k‘2  >  0.  The  kinematic  model  (11.80)  under  the  action  of  the  con¬ 
trol  law  (11.81),  (11.82)  asymptotically  converges  to  the  desired  configuration 
[p  7  <5]T  =  [0  0  0]T‘. 

The  proof  of  this  result  is  based  on  the  following  Lyapunov  candidate: 
v  =  \  (p2  +  72  +  h  S 2) , 

whose  time  derivative  along  the  closed-loop  system  trajectories 

V  =  —ki  cos  27  p2  —  k2  72 

is  negative  semi-definite.  As  a  consequence,  V  tends  to  a  limit  value  and  the 
system  state  is  bounded.  It  can  also  be  shown  that  V  is  bounded,  so  that  V 
is  uniformly  continuous.  In  view  of  Barbalat  lemma,  it  can  be  inferred  that 
V  tends  to  zero,  and  likewise  do  p  and  7.  Further  analysis  of  the  closed-loop 
system  leads  to  concluding  that  5  converges  to  zero  as  well. 

Note  that  angles  7  and  S  are  undefined  for  x  =  y  =  0.  They  are,  however, 
always  well-defined  during  the  motion  of  the  unicycle,  and  asymptotically  tend 
to  the  desired  zero  value. 

11  It  is  easy  to  verify  that  the  expression  (11.81)  for  v  coincides  with  (11.78),  the 
driving  velocity  prescribed  by  the  Cartesian  regulation  scheme,  except  for  the 
presence  of  p,  whose  effect  is  to  modulate  v  according  to  the  distance  of  the  robot 
from  the  destination.  As  for  the  steering  velocity,  (11.82)  differs  from  (11.79)  for 
the  presence  of  the  second  term,  that  contains  the  orientation  error  6  (through 
<5)  in  addition  to  the  pointing  error. 
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It  should  be  noted  that  the  control  law  (11.81),  (11.82),  once  mapped  back 
to  the  original  coordinates,  is  discontinuous  at  the  origin  of  the  configuration 
space  C.  As  a  matter  of  fact,  it  can  be  proven  that  any  feedback  law  that  can 
regulate  the  posture  of  the  unicycle  must  be  necessarily  discontinuous  with 
respect  to  the  state  and/or  time- varying. 12 


Simulations 

To  illustrate  the  characteristics  of  the  above  regulation  schemes,  simulation 
results  are  now  presented  for  two  parking  manoeuvres  performed  in  feedback 
by  a  unicycle  mobile  robot. 

Figure  11.19  shows  the  robot  trajectories  produced  by  the  Cartesian  regu¬ 
lator  (11.78),  (11.79),  with  k\  =  1  and  =  3,  for  two  different  initial  config¬ 
urations.  Note  that  the  final  orientation  of  the  robot  varies  with  the  approach 
direction,  and  that  the  unicycle  reaches  the  destination  in  forward  motion, 
after  inverting  its  motion  at  most  once  (like  in  the  second  manoeuvre).  It  is 
possible  to  prove  that  such  behaviour  is  general  with  this  controller. 

The  results  of  the  application  of  the  posture  regulator  (11.81),  (11.82) 
starting  from  the  same  initial  conditions  are  shown  in  Fig.  11.20.  The  gains 
have  been  set  to  k\  =  1,  k2  =  2.5  and  k3  =  3.  The  trajectories  obtained  are 
quite  similar  to  the  previous  ones,  but  as  expected  the  orientation  is  driven 
to  zero  as  well.  As  before,  the  final  approach  to  the  destination  is  always  in 
forward  motion,  with  at  most  one  motion  inversion  in  the  transient  phase. 


11.7  Odometric  Localization 

The  implementation  of  any  feedback  controller  requires  the  availability  of  the 
robot  configuration  at  each  time  instant.  Unlike  the  case  of  manipulators, 
in  which  the  joint  encoders  provide  a  direct  measurement  of  the  configura¬ 
tion,  mobile  robots  are  equipped  with  incremental  encoders  that  measure  the 
rotation  of  the  wheels,  but  not  directly  the  position  and  orientation  of  the 
vehicle  with  respect  to  a  fixed  world  frame.  It  is  therefore  necessary  to  devise 
a  localization  procedure  that  estimates  in  real  time  the  robot  configuration. 

Consider  a  unicycle  moving  under  the  action  of  velocity  commands  v  and 
io  that  are  constant  within  each  sampling  interval.  This  assumption,  which 


12  This  result,  which  actually  applies  to  all  nonholonomic  robots,  derives  from  the 
application  of  a  necessary  condition  ( Brockett  theorem )  for  the  smooth  stabi- 
lizability  of  control  systems.  In  the  particular  case  of  a  driftless  system  of  the 
form  (11.10),  in  which  there  are  fewer  inputs  than  states  and  the  input  vector 
fields  are  linearly  independent,  such  a  condition  is  violated  and  no  control  law 
that  is  continuous  in  the  state  q  can  asymptotically  stabilize  an  equilibrium  point. 
Brockett  theorem  does  not  apply  to  time-varying  stabilizing  controllers  that  may 
thus  be  continuous  in  q. 


11.7  Odometric  Localization 


515 


-2-1012 

[m] 


_ j 

\ 

L 1 

-2-1012 

[m] 


Fig.  11.19.  Regulation  to  the  origin  of  the  Cartesian  position  of  the  unicycle  with 
the  controller  (11.78),  (11.79),  for  two  different  initial  configurations 
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Fig.  11.20.  Regulation  to  the  origin  of  the  posture  of  the  unicycle  with  the  con¬ 
troller  (11.81),  (11.82),  for  two  different  initial  configurations 


is  generally  satisfied13  in  digital  control  implementations,  implies  that  during 
the  interval  the  robot  moves  along  an  arc  of  circle  of  radius  R  =  vk/tok,  which 
degenerates  to  a  line  segment  if  u>k  =  0.  Assume  that  the  robot  configuration 
q(tk)  =  qk  at  the  sampling  time  tk  is  known,  together  with  the  value  of 
the  velocity  inputs  Vk  and  cok  applied  in  the  interval  [tk,tk+i)-  The  value 
of  the  configuration  variables  qk+i  at  the  sampling  time  tk+\  can  then  be 
reconstructed  by  forward  integration  of  the  kinematic  model  (11.13). 

A  first  possibility  is  to  use  the  approximate  formulae 

Xk+i  =  xk  +  vk  Ts  cos  8k 

Uk+ 1  =  Uk  +  Vk  Ts  sin  8k  (11.83) 

8k+ 1  —  8k  iok  Ts , 

where  Ts  =  tk+ i  —tk  is  the  duration  of  the  sampling  interval.  These  equations, 
which  correspond  to  the  use  of  the  Euler  method  for  numerical  integration 

13  In  particular,  this  is  certainly  true  if  the  velocity  commands  computed  by  the 
digital  controller  are  converted  to  control  inputs  for  the  robot  through  a  zero- 
order  hold  (ZOH). 
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of  (11.13),  introduce  an  error  in  the  computation  of  xk+  i  and  yk+i,  that  is 
performed  as  if  the  orientation  9 k  remained  constant  throughout  the  interval. 
This  error  becomes  smaller  as  Ts  is  decreased.  The  third  formula  instead  is 
exact. 

If  the  accuracy  of  the  Euler  method  proves  to  be  inadequate,  one  may  use 
the  following  estimate  with  the  same  Ts: 

(  tUk  Ta  \ 

Xk+i  =  xk  +  vk  Ts  cos  I  9k  H - —  I 

Uk+i  =  Vk  +  vk  Ts  sin  (11.84) 

9k-\- i  —  9k  T  x)k  Ts, 

corresponding  to  the  adoption  of  the  second-order  Runge-Kutta  integration 
method.  Note  how  the  first  two  formulae  use  the  average  value  of  the  unicycle 
orientation  in  the  sampling  interval. 

To  obtain  an  exact  reconstruction  of  qfe+1  under  the  assumption  of  con¬ 
stant  velocity  inputs  within  the  sampling  interval  one  may  use  simple  geomet¬ 
ric  arguments  or  exploit  the  transformability  of  the  unicycle  kinematic  model 
in  the  chained  form  (11.25).  As  already  seen,  this  form  is  easily  integrable 
in  closed  form,  leading  to  an  exact  expression  for  zk+ 1-  The  configuration 
qk+ 1  can  then  be  computed  by  inverting  the  coordinate  and  input  transfor¬ 
mations  (11.23)  and  (11.24).  This  procedure,  which  can  be  generalized  to  any 
mobile  robot  that  can  be  put  in  chained  form,  provides  the  formulae: 

Xk+i  =  xk  +  —  (sin6*fc+i  -  sin  9k) 

^ k 

'Vjq 

2/fc+i  =  Vk  ~  —  (cos6»fc+i  -  cos  9k)  (11.85) 

Qk+i  =  9k  +  x>k  Ts. 

Note  that  the  first  two  are  still  defined  for  Uk  =  0;  in  this  case,  they  coincide 
with  the  corresponding  formulae  of  Euler  and  Runge-Kutta  methods  (which 
are  exact  over  line  segments).  In  the  implementation,  however,  it  is  necessary 
to  handle  this  situation  with  a  conditional  instruction. 

Figure  11.21  allows  a  comparison  among  the  configurations  qfc+1  recon¬ 
structed  via  the  three  aforementioned  integration  methods.  In  practice,  the 
difference  is  obviously  much  less  dramatic,  and  tends  to  disappear  as  the  du¬ 
ration  Ts  of  the  sampling  interval  is  decreased. 

In  the  previous  formulae  it  has  been  assumed  that  the  velocity  inputs  Vk 
and  Uk  applied  in  the  sampling  interval  are  available.  In  view  of  the  non¬ 
ideality  of  any  actuation  system,  rather  than  relying  on  the  ‘commanded’ 
values,  it  is  convenient  to  reconstruct  vk  and  uy,  using  the  robot  proprioceptive 
sensors.  First  of  all,  note  that 

Ufc  _  As 
LUk  A9  ’ 


vkTs  =  As  ukTa  =  A9 


(11.86) 
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Fig.  11.21.  Odometric  localization  for  a  unicycle  moving  along  an  elementary  tract 
corresponding  to  an  arc  of  circle;  left:  integration  via  Euler  method,  centre:  integra¬ 
tion  via  Runge-Kutta  method,  right:  exact  integration 


where  As  is  the  length  of  the  elementary  tract  travelled  by  the  robot  and  Ad 
is  the  total  variation  of  orientation  along  the  tract.  For  example,  in  the  case 
of  a  differential  drive  unicycle,  denote  by  A(f> r  and  A<f>L  the  rotation  of  the 
right  and  left  wheel,  respectively,  as  measured  by  the  incremental  encoders 
during  the  sampling  interval.  From  (11.14)  one  easily  finds 

As  =  ^  ( A<j>R  +  A(fL)  AO  =  ^  {A(f>R  -  A(j>L ) 

that,  used  in  (11.86),  allow  the  implementation  of  all  the  previous  formulae 
for  the  reconstruction  of  qk+1. 

The  forward  integration  of  the  kinematic  model  using  the  velocity  com¬ 
mands  reconstructed  via  the  proprioceptive  sensors  —  the  encoders  of  the 
wheel  actuators  —  is  referred  to  as  odometric  localization  or  dead  reckoning , 
the  latter  being  a  term  of  uncertain  etymology  used  in  marine  navigation. 
This  method,  relying  on  the  iterated  use  of  the  previous  formulae  starting 
from  an  estimate  of  the  initial  configuration,  provides  an  estimate  whose  ac¬ 
curacy  cannot  be  better  than  that  of  qQ.  In  any  case,  odometric  localization 
—  independently  from  the  adopted  integration  method  —  is  subject  in  prac¬ 
tice  to  an  error  that  grows  over  time  (drift)  and  quickly  becomes  significant 
over  sufficiently  long  paths.  This  error  is  the  result  of  several  causes,  that 
include  wheel  slippage,  inaccuracy  in  the  calibration  of  kinematic  parameters 
(e.g.,  the  radius  of  the  wheels),  as  well  as  the  numerical  error  introduced  by 
the  integration  method,  if  Euler  or  Runge-Kutta  methods  are  used.  It  should 
also  be  noted  that,  once  an  odometric  localization  technique  has  been  chosen, 
its  performance  also  depends  on  the  specific  kinematic  arrangement  of  the 
robot;  for  example,  differential  drive  is  usually  better  than  synchro  drive  in 
this  respect. 

A  more  robust  solution  is  localization  based  on  exteroceptive  sensors.  For 
example,  this  kind  of  approach  can  be  adopted  when  the  robot  is  equipped 
with  proximity  sensors  (such  as  a  laser  range  finder)  and  knows  a  map  of 
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the  workspace,  either  given  in  advance  or  built  by  the  robot  itself  during  the 
motion.  It  is  then  possible  to  correct  the  estimate  provided  by  dead  reckoning 
by  comparing  the  expected  measures  of  the  exteroceptive  sensors  with  the 
actual  readings.  These  techniques,  which  make  use  of  tools  from  Bayesian 
estimation  theory  such  as  the  Extended  Kalman  Filter  or  the  Particle  Filter, 
provide  greater  accuracy  than  pure  odometric  localization  and  are  therefore 
essential  in  navigation  tasks  over  long  paths. 
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Problems 

11.1.  Consider  the  mobile  robot  obtained  by  connecting  N  trailers  to  a  rear- 
wheel  drive  tricycle.  Each  trailer  is  a  rigid  body  with  an  axle  carrying  two  fixed 
wheels,  that  can  be  assimilated  to  a  single  wheel  located  at  the  midpoint  of  the 
axle,  and  is  hinged  to  the  midpoint  of  the  preceding  axle  through  a  revolute 
joint.  Find  a  set  of  generalized  coordinates  for  the  robot. 
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11.2.  Consider  an  omnidirectional  mobile  robot  having  three  Mecanum  wheels 
placed  at  the  vertices  of  an  equilateral  triangle,  each  oriented  in  the  direction 
orthogonal  to  the  bisectrix  of  its  angle.  Let  q\  and  q2  be  the  Cartesian  coor¬ 
dinates  of  the  centre  of  the  robot,  <73  the  vehicle  orientation,  while  <74,  (75,  and 
qe  represent  the  angle  of  rotation  of  each  wheel  around  its  axis.  Also,  denote 
by  r  the  radius  of  the  wheels  and  by  t  the  distance  between  the  centre  of  the 
robot  and  the  centre  of  each  wheel.  This  mechanical  system  is  subject  to  the 
following  Pfaffian  constraints: 
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and  A 2  =  r  1 3.  Show  that  this  set  of  constraints  is  partially  integrable,  and 
that  in  particular  the  orientation  of  the  vehicle  is  a  linear  function  of  the  wheel 
rotation  angles.  [Hint:  add  the  kinematic  constraints  side-by-side.] 


11.3.  Show  that,  for  a  single  kinematic  constraint  in  the  form  (11.7),  the 
integrability  condition  expressed  by  (11.9)  coincides  with  the  involutivity  of 
the  distribution  A  =  span{gl5 , . . ,  gn_{\  associated  with  the  input  vector 
fields  of  the  corresponding  kinematic  model. 


11.4.  Using  the  controllability  condition  (11.11),  show  that  a  set  of  Pfaffian 
constraints  that  does  not  depend  on  the  generalized  coordinates  is  always 
integrable. 


11.5.  With  reference  to  the  kinematic  model  (11.13)  of  the  unicycle,  consider 
the  following  sequence  of  velocity  inputs: 


v(t)  =  1  ui(t)  =  0,  t  €  [0,  e) 

v(t)  =  0  uj(t)  =  1  t  £  [e,  2e) 

v(t)  =  —1  oj(t)  =  0,  t  €  [2e,3e) 

v(t)  =  0  cj  =  —  1  t  €  [3e,4e) 


By  forward  integration  of  the  model  equations,  show  that  when  e  is  infinites¬ 
imal  the  displacement  of  the  unicycle  in  configuration  space  is  aligned  with 
the  Lie  bracket  of  the  input  vector  fields. 

11.6.  Prove  the  relationships  (11.14)  that  allow  the  transformation  of  the 
velocity  inputs  of  a  differential  drive  vehicle  to  those  of  the  equivalent  unicycle. 
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[Hint:  for  the  velocity  of  the  midpoint  of  the  segment  joining  the  two  wheels, 
it  is  sufficient  to  differentiate  with  respect  to  time  the  Cartesian  coordinates 
of  such  a  point  expressed  as  a  function  of  the  coordinates  of  the  wheel  centres. 
As  for  u>,  use  the  formula  (B.4)  for  the  composition  of  the  velocities  of  a  rigid 
body.] 

11.7.  For  a  generic  configuration  of  a  bicycle  mobile  robot,  determine  the 
Cartesian  position  of  the  instantaneous  centre  of  rotation,  and  derive  the 
expression  of  the  angular  velocity  of  the  body  as  a  function  of  the  robot 
configuration  q  and  of  the  modulus  of  the  velocity  of  rear  wheel  centre.  In  the 
particular  case  of  the  rear- wheel  drive  bicycle,  show  that  such  expression  is 
consistent  with  the  evolution  of  9  predicted  by  the  kinematic  model  (11.19). 
Moreover,  compute  the  velocity  wp  of  a  generic  point  P  on  the  robot  chassis. 

11.8.  Derive  the  kinematic  model  of  the  tricycle  robot  towing  N  trailers  con¬ 
sidered  in  Problem  11.1.  Denote  by  t  the  distance  between  the  front  wheel 
and  rear  wheel  axle  of  the  tricycle,  and  by  U  the  joint-to-joint  length  of  the 
*-th  trailer. 

11.9.  Consider  a  differential  drive  robot  whose  angular  speed  inputs  —  one 
for  the  right  wheel  and  one  for  the  left  wheel  —  are  subject  to  the  following 
bounds: 

|wi(t)|  <  Wrl  Vf, 

that  correspond  to  a  square  admissible  region  in  the  plane.  Derive  the 

expression  of  the  resulting  constraints  for  the  driving  and  steering  velocity  v 
and  u!  of  the  equivalent  unicycle  model.  In  particular,  show  that  the  admissible 
region  in  the  v,u)  plane  is  a  rhombus. 

11.10.  Compute  the  expression  of  matrix  D(A)  and  vector  d(zi,Zf,A) 
in  (11.50)  for  the  case  of  the  (2,4)  chained  form. 

11.11.  Modify  the  path  planning  scheme  based  on  parameterized  inputs  so 
as  to  obtain  Sf  =  1. 

11.12.  Show  that  the  path  planning  schemes  based  on  polynomials  of  different 
degree  and  on  parameterized  inputs  give  exactly  the  same  result  in  the  case 
of  the  (2,3)  chained  form. 

11.13.  Implement  in  a  computer  program  the  path  planning  method  for  a 

unicycle  based  on  cubic  Cartesian  polynomials.  Use  the  program  to  plan  a 
path  leading  the  robot  from  the  configuration  qi  =  [0  0  0]T  [m,m,rad]  to 

the  configuration  q  ^  =  [2  1  7t/2]t  [m,m,rad].  Then,  determine  a  timing 

law  over  the  path  so  as  to  satisfy  the  following  velocity  bounds: 

|u(t)|<lm/s  |w(t):|  <  1  rad/s  Vi. 
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11.14.  Formulate  the  trajectory  tracking  problem  for  a  (2,3)  chained  form 
and  derive  the  corresponding  error  dynamics.  Derive  a  feedback  controller 
using  the  linear  approximation  around  the  reference  trajectory. 

11.15.  Consider  the  kinematic  model  (11.19)  of  the  rear-wheel  drive  bicycle. 
In  analogy  to  the  case  of  the  unicycle,  identify  two  outputs  yi,  2/2  for  which 
it  is  possible  to  perform  a  static  input/output  linearization.  [Hint:  consider  a 
point  P  on  the  line  passing  through  the  centre  of  the  front  wheel  and  oriented 
as  the  wheel  itself.] 

11.16.  Implement  in  a  computer  program  the  Cartesian  regulator  (11.78), 
(11.79),  including  a  modification  to  allow  the  unicycle  to  reach  the  origin 
either  in  forward  or  in  backward  motion.  [Hint:  modify  (11.79)  so  as  to  force 
the  robot  to  orient  itself  as  vector  ep  or  vector  —ep,  depending  on  which  choice 
is  the  most  convenient.] 

11.17.  Prove  formulae  (11.85)  for  the  exact  odometric  localization  of  a  uni¬ 
cycle  under  velocity  inputs  that  are  constant  in  the  sampling  interval. 

11.18.  Implement  in  a  computer  program  the  unicycle  posture  regulator 
based  on  polar  coordinates,  with  the  state  feedback  computed  through  the 
Runge-Kutta  odometric  localization  method. 
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The  trajectory  planning  methods  presented  in  Chaps.  4  and  11,  respectively 
for  manipulators  and  mobile  robots,  operate  under  the  simplifying  assumption 
that  the  workspace  is  empty.  In  the  presence  of  obstacles,  it  is  necessary  to  plan 
motions  that  enable  the  robot  to  execute  the  assigned  task  without  colliding 
with  them.  This  problem,  referred  to  as  motion  planning,  is  the  subject  of 
this  chapter.  After  defining  a  canonical  version  of  the  problem,  the  concept  of 
configuration  space  is  introduced  in  order  to  achieve  an  efficient  formulation.  A 
selection  of  representative  planning  techniques  is  then  presented.  The  method 
based  on  the  notion  of  retraction  characterizes  the  connectivity  of  the  free 
configuration  space  using  a  roadmap ,  i.e.,  a  set  of  collision-free  paths,  while 
the  cell  decomposition  method  identifies  a  network  of  channels  with  the  same 
property.  The  PRM  and  bidirectional  RRT  techniques  are  probabilistic  in 
nature  and  rely  on  the  randomized  sampling  of  the  configuration  space  and 
the  memorization  of  those  samples  that  do  not  cause  a  collision  between  the 
robot  and  the  obstacles.  The  artificial  potential  method  is  also  described  as 
a  heuristic  approach  particularly  suited  to  on-line  planning  problems,  where 
the  geometry  of  the  workspace  obstacles  is  unknown  in  advance.  The  chapter 
ends  with  a  discussion  of  the  application  of  the  presented  planning  methods 
to  the  robot  manipulator  case. 


12.1  The  Canonical  Problem 

Robotic  systems  are  expected  to  perform  tasks  in  a  workspace  that  is  often 
populated  by  physical  objects,  which  represent  an  obstacle  to  their  motion. 
For  example,  a  manipulator  working  in  a  robotized  cell  must  avoid  collision 
with  its  static  structures,  as  well  as  with  other  moving  objects  that  may  access 
it,  such  as  other  manipulators.  Similarly,  a  mobile  robot  carrying  baggage  in 
an  airport  has  to  navigate  among  obstacles  that  may  be  fixed  (fittings,  con¬ 
veyor  belts,  construction  elements)  or  mobile  (passengers,  workers).  Planning 
a  motion  amounts  then  to  deciding  which  path  the  robot  must  follow  in  order 
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to  execute  a  transfer  task  from  an  initial  to  a  final  posture  without  collid¬ 
ing  with  the  obstacles.  Clearly,  one  would  like  to  endow  the  robot  with  the 
capability  of  autonomously  planning  its  motion,  starting  from  a  high-level  de¬ 
scription  of  the  task  provided  by  the  user  and  a  geometric  characterization  of 
the  workspace,  either  made  available  entirely  in  advance  (off-line  planning)  or 
gathered  by  the  robot  itself  during  the  motion  by  means  of  on-board  sensors 
(on-line  planning). 

However,  developing  automatic  methods  for  motion  planning  is  a  very 
difficult  endeavour.  In  fact,  the  spatial  reasoning  that  humans  instinctively 
use  to  move  safely  among  obstacles  has  proven  hard  to  replicate  and  codify  in 
an  algorithm  that  can  be  executed  by  a  robot.  To  this  date,  motion  planning  is 
still  an  active  topic  of  research,  with  contributions  coming  from  different  areas 
such  as  algorithm  theory,  computational  geometry  and  automatic  control. 

To  address  the  study  of  methods  for  motion  planning,  it  is  convenient  to 
introduce  a  version  of  the  problem  that  highlights  its  fundamental  issues.  The 
canonical  problem  of  motion  planning  is  then  formulated  as  follows. 

Consider  a  robot  B ,  which  may  consist  of  a  single  rigid  body  (mobile  robot) 
or  of  a  kinematic  chain  whose  base  is  either  fixed  (standard  manipulator)  or 
mobile  (mobile  robot  with  trailers  or  mobile  manipulator).  The  robot  moves 
in  a  Euclidean  space  W  =  H1V,  with  N  =  2  or  3,  called  workspace.  Let 
O i, . . .  ,Op  be  the  obstacles,  i.e. ,  fixed  rigid  objects  in  W.  It  is  assumed  that 
both  the  geometry  of  B,  Oi, ...  ,Op  and  the  pose  of  0\,...,0P  in  W  are 
known.  Moreover,  it  is  supposed  that  B  is  free- flying ,  that  is,  the  robot  is 
not  subject  to  any  kinematic  constraint.  The  motion  planning  problem  is  the 
following:  given  an  initial  and  a  final  posture  of  B  in  W,  find  if  exists  a  path, 
i.e.,  a  continuous  sequence  of  postures,  that  drives  the  robot  between  the 
two  postures  while  avoiding  collisions  (including  contacts)  between  B  and  the 
obstacles  0±, ... ,  Op\  report  a  failure  if  such  a  path  does  not  exist. 

In  the  particular  case  in  which  the  robot  is  a  single  body  moving  in  IR2, 
the  canonical  motion  planning  problem  is  also  known  as  the  piano  movers’ 
problem,  as  it  captures  the  difficulties  faced  by  movers  when  manoeuvring  a 
piano  (without  lifting  it)  among  obstacles.  The  generalized  movers’  problem 
is  the  canonical  problem  for  a  single-body  robot  moving  in  IR3. 

Clearly,  some  of  the  hypotheses  of  the  canonical  problem  may  not  be  sat¬ 
isfied  in  applications.  For  example,  the  assumption  that  the  robot  is  the  only 
object  in  motion  in  the  workspace  rules  out  the  relevant  case  of  moving  obsta¬ 
cles  (e.g.,  other  robots).  Advance  knowledge  of  obstacle  geometry  and  place¬ 
ment  is  another  strong  assumption:  especially  in  unstructured  environments, 
which  are  not  purposely  designed  to  accommodate  robots,  the  robot  itself  is 
typically  in  charge  of  detecting  obstacles  by  means  of  its  sensors,  and  the  plan¬ 
ning  problem  must  therefore  be  solved  on-line  during  the  motion.  Moreover,  as 
shown  in  Chap.  11,  the  free- flying  robot  hypothesis  does  not  hold  in  nonholo- 
nomic  mechanical  systems,  which  cannot  move  along  arbitrary  paths  in  the 
workspace.  Finally,  manipulation  and  assembly  problems  are  excluded  from 
the  canonical  formulation  since  they  invariably  involve  contacts  between  rigid 
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bodies.  As  a  matter  of  fact,  all  the  above  assumptions  are  introduced  in  order 
to  reduce  motion  planning  to  the  purely  geometrical  -  but  still  quite  difficult 
problem  of  generating  a  collision-free  path.  However,  many  methods  that 
successfully  solve  this  simplified  version  of  the  problem  lend  themselves  to  an 
extension  to  more  general  versions. 

The  notion  of  configuration  space  is  essential  to  obtain  a  more  convenient 
formulation  of  the  canonical  motion  planning  problem,  as  well  as  to  envisage 
approaches  to  its  solution. 


12.2  Configuration  Space 

A  very  effective  scheme  for  motion  planning  is  obtained  by  representing  the 
robot  as  a  mobile  point  in  an  appropriate  space,  where  the  images  of  the 
workspace  obstacles  are  also  reported.  To  this  end,  it  is  natural  to  refer  to  the 
generalized  coordinates  of  the  mechanical  system,  whose  value  identifies  the 
configuration  of  the  robot  (see  Sect.  B.4).  This  associates  to  each  posture  of  the 
latter  a  point  in  the  configuration  space  C,  i.e.,  the  set  of  all  the  configurations 
that  the  robot  can  assume. 

Generalized  coordinates  of  robots  are  essentially  of  two  types.  Cartesian 
coordinates  are  used  to  describe  the  position  of  selected  points  on  the  links  of 
the  kinematic  chain  and  take  value  in  Euclidean  spaces.  Angular  coordinates 
are  used  to  represent  the  orientations  of  the  bodies;  independently  from  the 
adopted  representation  (rotation  matrices,  Euler  angles,  quaternions),  they 
take  values  in  50(to)  (to  =  2,  3),  the  special  orthonormal  group  of  real  (toxto) 
matrices  with  orthonormal  columns  and  determinant  equal  to  1  (see  Sect.  2.2). 
It  is  well  known  that  a  minimal  parameterization  of  50(?n)  requires  m(m  — 
1) /2  parameters.  The  configuration  space  of  a  robot  is  then  obtained  in  general 
as  a  Cartesian  product  of  these  spaces. 

Some  examples  of  configuration  spaces  are  presented  below: 

•  The  configuration  of  a  polygonal  mobile  robot  in  W  =  IR2  is  described 
by  the  position  of  a  representative  point  on  the  body  (e.g.,  a  vertex)  and 
by  the  orientation  of  the  polygon,  both  expressed  with  respect  to  a  fixed 
reference  frame.  The  configuration  space  C  is  then  IR2  x  50( 2),  whose 
dimension  is  n  =  3. 

•  For  a  polyhedral  mobile  robot  in  W  =  IR3,  the  configuration  space  C  is 
IR3  x  50(3),  whose  dimension  is  n  =  6. 

•  For  a  fixed-base  planar  manipulator  with  n  revolute  joints,  the  configura¬ 
tion  space  is  a  subset  of  (IR2  x  50(2))™.  The  dimension  of  C  equals  the 
dimension  of  (IR2  x  50(2))"  minus  the  number  of  constraints  due  to  the 
presence  of  the  joints,  i.e.,  3n—2n  =  n.  In  fact,  in  a  planar  kinematic  chain, 
each  joint  imposes  two  holonomic  constraints  on  the  following  body. 

•  For  a  fixed-base  spatial  manipulator  with  n  revolute  joints,  the  configura¬ 
tion  space  is  a  subset  of  (IR3  x  50(3))™.  Since  in  this  case  each  joint  imposes 
five  constraints  on  the  following  body,  the  dimension  of  C  is  6 n  —  5n  =  n. 
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Fig.  12.1.  The  configuration  space  of  a  2R  manipulator;  left:  a  locally  valid  rep¬ 
resentation  as  a  subset  of  IR2,  right:  a  topologically  correct  representation  as  a 
two-dimensional  torus 


•  For  a  unicycle-like  vehicle  with  a  trailer  in  IR2,  the  configuration  space  is 
a  subset  of  (IR2  x  50(2))  x  (IR2  x  50(2)).  If  the  trailer  is  connected  to  the 
unicycle  by  a  revolute  joint,  the  configuration  of  the  robot  can  be  described 
by  the  position  and  orientation  of  the  unicycle  and  the  orientation  of  the 
trailer.  The  dimension  of  C  is  therefore  n  =  4. 

If  n  is  the  dimension  of  C,  a  configuration  in  C  can  be  described  by  a 
vector  q  £  IR™.  However,  this  description  is  only  valid  locally:  the  geometric 
structure  of  the  configuration  space  C  is  in  general  more  complex  than  that 
of  a  Euclidean  space,  as  shown  in  the  following  example. 


Example  12.1 

Consider  the  planar  manipulator  with  two  revolute  joints  (2R  manipulator)  of 
Fig.  2.14.  The  configuration  space  has  dimension  2,  and  may  be  locally  represented 
by  IR2,  or  more  precisely  by  its  subset 

Q  =  {<7  =  ('ll,  9a)  :  Qi  &  [0,  2t r),  q2  £  [0,  2tt)}. 

This  guarantees  that  the  representation  is  injective,  i.e.,  that  a  single  value  of  q 
exists  for  each  manipulator  posture.  However,  this  representation  is  not  topologically 
correct:  for  example,  the  configurations  denoted  as  qA  and  qB  in  Fig.  12.1,  left,  which 
correspond  to  manipulator  postures  that  are  ‘close’  in  the  workspace  W,  appear  to 
be  ‘far’  in  Q.  To  take  this  into  account,  one  should  ‘fold’  the  square  Q  onto  itself 
(so  as  to  make  opposite  sides  meet)  in  sequence  along  its  two  axes.  This  procedure 
generates  a  ring,  properly  called  torus,  which  can  be  visualized  as  a  two-dimensional 
surface  immersed  in  IR3  (Fig.  12.1,  right).  The  correct  expression  of  this  space  is 
50(2)  x  50(2). 
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When  the  configuration  of  a  robot  (either  articulated  or  mobile)  includes 
angular  generalized  coordinates,  its  configuration  space  is  properly  described 
as  an  n-dimensional  manifold ,  i.e.,  a  space  in  which  a  neighbourhood  of  a 
point  can  be  put  in  correspondence  with  IR"  through  a  continuous  bijective 
function  whose  inverse  is  also  continuous  (a  homeomorphism) . 


12.2.1  Distance 

Having  discussed  the  nature  of  the  robot  configuration  space,  it  is  useful  to 
define  a  distance  function  in  C.  In  fact,  the  planning  methods  that  will  be 
discussed  in  the  following  make  use  of  this  notion. 

Given  a  configuration  q,  let  B(q)  be  the  subset  of  the  workspace  W  occu¬ 
pied  by  the  robot  £>,  and  p(q )  be  the  position  in  W  of  a  point  p  on  B.  Intuition 
suggests  that  the  distance  between  two  configurations  qA  and  qB  should  go 
to  zero  when  the  two  regions  B(qA)  and  B(qB)  tend  to  coincide.  A  definition 
that  satisfies  this  property  is 

di{qA,  Qb)  =  max  Wp(Qa)  ~  p(Qb) II,  d12-1) 

pGo 

where  ||  •  ||  denotes  the  Euclidean  distance  in  W  =  IR^.  In  other  words,  the 
distance  between  two  configurations  in  C  is  the  maximum  displacement  in  W 
they  induce  on  a  point,  as  the  point  moves  all  over  the  robot. 

However,  the  use  of  function  d\  is  cumbersome,  because  it  requires  char¬ 
acterizing  the  volume  occupied  by  the  robot  in  the  two  configurations  and  the 
computation  of  the  maximum  distance  in  W  between  corresponding  points. 
For  algorithmic  purposes,  the  simple  Euclidean  norm  is  often  chosen  as  a 
configuration  space  distance: 

MqA,QB)  =  \\Qa~  Qb\\-  (12-2) 

Nevertheless,  one  must  keep  in  mind  that  this  definition  is  appropriate  only 
when  C  is  a  Euclidean  space.  Going  back  to  Example  12.1,  it  is  easy  to  real¬ 
ize  that,  unlike  d\{qA,  qB),  the  Euclidean  norm  d2(qA ,  qB)  does  not  represent 
correctly  the  distance  on  the  torus.  A  possible  solution  is  to  modify  the  defini¬ 
tion  of  c?2  by  suitably  computing  differences  of  angular  generalized  coordinates 
(see  Problem  12.2). 


12.2.2  Obstacles 

In  order  to  characterize  paths  that  represent  a  solution  to  the  canonical  motion 
planning  problem  —  those  that  avoid  collisions  between  the  robot  and  the 
workspace  obstacles  —  it  is  necessary  to  build  the  ‘images’  of  the  obstacles  in 
the  configuration  space  of  the  robot. 

In  the  following,  it  is  assumed  that  the  obstacles  are  closed  (i.e.,  they 
contain  their  boundaries)  but  not  necessarily  limited  subsets  of  W.  Given  an 
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obstacle  O,  (i  =  1 ,p)  in  W,  its  image  in  configuration  space  C  is  called 
C-obstacle  and  is  defined  as 

COi  =  {q&C:B{q)nOl^®}.  (12.3) 

In  other  words,  COi  is  the  subset  of  configurations  that  cause  a  collision 
(including  simple  contacts)  between  the  robot  B  and  the  obstacle  Oi  in  the 
workspace.  The  union  of  all  C-obstacles 

p 

CO={ J  COi  (12.4) 

2=1 

defines  the  C-obstacle  region ,  while  its  complement 

Cfree  =  C-CO  =  {qeC  :  B{q)  fl  ( [J  O^  =  0}  (12.5) 

M=1  ' 

is  the  free  configuration  space,  that  is,  the  subset  of  robot  configurations  that 
do  not  cause  collision  with  the  obstacles.  A  path  in  configuration  space  is 
called  free  if  it  is  entirely  contained  in  Cfree- 

Although  C  in  itself  is  a  connected  space  —  given  two  arbitrary  configura¬ 
tion  there  exists  a  path  that  joins  them  —  the  free  configuration  space  Cfree 
may  not  be  connected  as  a  consequence  of  occlusions  due  to  C-obstacles.  Note 
also  that  the  assumption  of  free-flying  robot  in  the  canonical  problem  means 
that  the  robot  can  follow  any  path  in  the  free  configuration  space  Cfree- 

It  is  now  possible  to  give  a  more  compact  formulation  of  the  canonical  mo¬ 
tion  planning  problem.  Assume  that  the  initial  and  final  posture  of  the  robot 
B  in  W  are  mapped  to  the  corresponding  configurations  in  C,  respectively 
called  start  configuration  qs  and  goal  configuration  qg.  Planning  a  collision- 
free  motion  for  the  robot  means  then  generating  a  safe  path  between  q8  and 
qg  if  they  belong  to  the  same  connected  component  of  Cfree,  and  reporting  a 
failure  otherwise. 

12.2.3  Examples  of  Obstacles 

In  the  following,  the  C-obstacle  generation  procedure  is  presented  in  some 
representative  cases.  For  the  sake  of  simplicity,  it  is  assumed  that  obstacles  in 
W  are  either  polygonal  or  polyhedral. 


Example  12.2 

Consider  the  case  of  a  point  robot  B.  In  this  case,  the  configuration  of  the  robot 
is  described  by  the  coordinates  of  point  B  in  the  workspace  W  =  IRJV  and  the 
configuration  space  C  is  a  copy  of  W.  Similarly,  the  C-obstacles  are  copies  of  the 
obstacles  in  W. 
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Fig.  12.2.  C-obstacles  for  a  circular  robot  in  IR2;  left:  the  robot  B,  an  obstacle  Oi 
and  the  growing  procedure  for  building  C-obstacles,  right:  the  configuration  space  C 
and  the  C-obstacle  COi 


Example  12.3 

If  the  robot  is  a  sphere1  in  W  =  IR^,  its  configuration  can  be  described  by  the 
Cartesian  coordinates  of  a  representative  point,  e.g.,  its  centre  —  note  that  the 
orientation  of  the  sphere  is  irrelevant  for  collision  checking.  Therefore,  as  in  the 
previous  example,  the  configuration  space  C  is  a  copy  of  the  workspace  W.  However, 
the  C-obstacles  are  no  longer  simple  copies  of  the  obstacles  in  W,  and  they  must 
be  built  through  a  growing  procedure.  In  particular,  the  boundary  of  the  C-obstacle 
COi  is  the  locus  of  configurations  that  put  the  robot  is  in  contact  with  the  obstacle 
Oi,  and  it  can  be  obtained  as  the  surface  described  by  the  representative  point  as  the 
robot  slides  on  the  boundary  of  Oi.  As  a  consequence,  to  build  COi  it  is  sufficient 
to  grow  Oi  isotropically  by  the  radius  of  the  robot.  This  procedure  is  shown  in 
Fig.  12.2  for  the  case  N  =  2  (circular  robot  in  IR2);  in  this  case,  each  C-obstacle  is 
a  generalized  polygon,  i.e.,  a  planar  region  whose  boundary  consists  of  line  segments 
and/or  circular  arcs.  If  the  representative  point  of  the  robot  is  different  from  the 
centre  of  the  sphere,  the  growing  procedure  is  not  isotropic. 

Example  12.4 

Consider  now  the  case  of  a  polyhedral  robot  that  is  free  to  translate  (with  a  fixed 
orientation)  in  IR^.  Its  configuration  can  be  described  by  the  Cartesian  coordinates 
of  a  representative  point,  for  example  a  vertex  of  the  polyhedron.  Therefore,  the 
configuration  space  C  is  again  a  copy  of  IR^.  Again,  a  growing  procedure  must  be 
applied  to  the  workspace  obstacles  to  obtain  their  image  in  the  configuration  space. 
In  particular,  the  boundary  of  the  C-obstacle  COi  is  the  surface  described  by  the 
representative  point  of  the  robot  when  the  robot  B  slides  at  a  fixed  orientation  on 
the  boundary  of  Oi.  Figure  12.3  shows  this  procedure  for  the  case  N  —  2  (polygonal 
robot  in  IR2).  The  resulting  shape  of  COi  depends  on  the  position  of  the  represen¬ 
tative  point  on  the  robot,  but  in  any  case  the  C-obstacle  is  itself  a  polyhedron.  COi 
has  in  general  a  larger  number  of  vertices  than  Oi,  and  is  a  convex  polyhedron  pro¬ 
vided  that  B  and  Oi  are  convex.  Note  also  that,  although  the  result  of  the  growing 


1  For  simplicity,  the  term  sphere  will  be  used  in  Euclidean  spaces  of  arbitrary 
dimension  n  in  place  of  n -sphere. 
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Fig.  12.3.  C-obstacles  for  a  polygonal  robot  translating  in  IR2;  left:  the  robot  B,  an 
obstacle  Oi  and  the  growing  procedure  for  building  C-obstacles,  right :  the  configu¬ 
ration  space  C  and  the  C-obstacle  COi 


procedure  —  and  thus  the  shape  of  the  C-obstacles  —  depends  on  the  choice  of 
the  representative  point  on  the  robot,  all  the  obtained  planning  problems  in  con¬ 
figuration  space  are  equivalent.  In  particular,  the  existence  of  a  solution  for  any  of 
them  implied  the  existence  of  a  solution  for  all  the  others.  Moreover,  to  each  path  in 
configuration  space  that  solves  one  of  these  problems  corresponds  a  (different)  free 
path  in  any  of  the  others,  to  which  the  same  motion  of  the  robot  in  the  workspace 
is  associated. 

Example  12.5 

For  a  polyhedral  robot  that  can  translate  and  rotate  in  JRlV,  the  dimension  of  the 
configuration  space  is  increased  with  respect  to  the  previous  example,  because  it  is 
also  necessary  to  describe  the  orientation  DOFs.  For  example,  consider  the  case  of  a 
polygon  that  can  translate  and  rotate  in  1R2.  The  configuration  of  the  robot  can  be 
characterized  by  the  Cartesian  coordinates  of  a  representative  point  (for  example, 
a  vertex  of  the  polygon)  and  an  angular  coordinate  8  representing  the  orientation 
of  the  polygon  with  respect  to  a  fixed  reference  frame.  The  configuration  space  C  is 
then  IR2  x  50(2),  which  can  be  locally  represented  by  IR3.  To  build  the  image  in  C  of 
an  obstacle  Oi,  one  should  in  principle  repeat  the  procedure  illustrated  in  Fig.  12.3 
for  each  possible  value  of  the  robot  orientation  8.  The  C-obstacle  COi  is  the  volume 
generated  by  ‘stacking’  (in  the  direction  of  the  8  axis)  all  the  constant-orientation 
slices  thus  obtained. 

Example  12.6 

For  a  robot  manipulator  B  made  by  rigid  links  B\,  ...  ,  B„  connected  by  joints,  there 
exist  in  principle  two  kinds  of  C-obstacles:  those  that  represent  the  collision  between 
a  body  Bi  and  an  obstacle  Oj,  and  those  accounting  for  self-collisions,  i.e.,  inter¬ 
ference  between  two  links  Bi  and  Bj  of  the  kinematic  chain.  Even  considering  for 
simplicity  only  the  first  type,  the  procedure  for  building  C-obstacles  is  much  more 
complicated  than  in  the  previous  examples.  In  fact,  to  obtain  the  boundary  of  the 
C-obstacle  COi,  it  is  necessary  to  identify  through  appropriate  inverse  kinematics 
computations  all  the  configurations  that  bring  one  or  more  links  of  the  manipula¬ 
tor  B  in  contact  with  Oi.  Figure  12.4  shows  the  result  of  the  C-obstacle  building 
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Fig.  12.4.  C-obstacles  for  a  wire-frame  2R  manipulator  in  two  different  cases;  left: 
the  robot  and  the  obstacles  in  W  =  IR2,  right:  the  configuration  space  C  and  the 
C-obstacle  region  CO 


procedure  for  a  wire-frame  2R  manipulator  in  two  different  cases  (self-collisions  are 
not  considered);  note  how,  in  spite  of  the  simple  shape  of  the  obstacles  in  W,  the 
profile  of  the  C-obstacles  is  quite  complicated.  For  simplicity,  the  configuration  space 
has  been  represented  as  a  subset  (a  square)  of  IR2;  however,  to  correctly  visualize 
the  C-obstacles,  one  should  keep  in  mind  that  the  correct  representation  of  C  is  a 
two-dimensional  torus,  so  that  the  upper/lower  and  left/right  sides  of  the  square 
are  actually  coincident.  Note  also  that  in  the  first  case  (top  of  Fig.  12.4)  the  images 
of  the  two  obstacles  in  the  upper  right  corner  of  W  merge  in  a  single  C-obstacle, 
and  the  free  configuration  space  Cfree  consists  of  a  single  connected  component.  In 
the  second  case  (bottom  of  Fig.  12.4)  Cfree  is  instead  partitioned  into  three  disjoint 
connected  components. 
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Whatever  the  nature  of  the  robot,  an  algebraic  model  of  the  workspace 
obstacles  is  needed  (e.g.,  derived  from  a  CAD  model  of  the  workspace)  to 
compute  exactly  the  C-obstacle  region  CO.  However,  except  for  the  most  ele¬ 
mentary  cases,  the  procedures  for  generating  CO  are  extremely  complex.  As 
a  consequence,  it  is  often  convenient  to  resort  to  approximate  representations 
of  CO.  A  simple  (although  computationally  intensive)  way  to  build  such  a 
representation  is  to  extract  configuration  samples  from  C  using  a  regular  grid, 
compute  the  corresponding  volume  occupied  by  the  robot  via  direct  kinemat¬ 
ics,  and  finally  identify  through  collision  checking 2  those  samples  that  bring 
the  robot  to  collide  with  obstacles.  These  samples  can  be  considered  as  a 
discrete  representation  of  CO ,  whose  accuracy  can  be  improved  at  will  by 
increasing  the  resolution  of  the  grid  in  C. 

Some  of  the  motion  planning  methods  that  will  be  presented  in  this  chapter 
do  not  require  an  explicit  computation  of  the  C-obstacle  region.  In  particular, 
this  is  true  for  the  probabilistic  planners  described  in  Sect.  12.5,  and  for  the 
technique  based  on  artificial  potential  fields  and  control  points  discussed  in 
Sect.  12.7. 


12.3  Planning  via  Retraction 

The  basic  idea  of  motion  planning  via  retraction  is  to  represent  the  free  con¬ 
figuration  space  by  means  of  a  roadmap  1Z  C  Cfree,  he.,  a  network  of  paths 
that  describe  adequately  the  connectivity  of  Cflee-  The  solution  of  a  particular 
instance  of  a  motion  planning  problem  is  then  obtained  by  connecting  ( retract¬ 
ing )  to  the  roadmap  the  start  configuration  qs  and  the  goal  configuration  qg, 
and  finding  a  path  on  1Z  between  the  two  connection  points.  Depending  on 
the  type  of  roadmap,  and  on  the  retraction  procedure,  this  general  approach 
leads  to  different  planning  methods.  One  of  these  is  described  in  the  follow¬ 
ing,  under  the  simplifying  assumption  that  Cfree  is  a  limited  subset  of  C  =  IR2 
and  is  polygonal ,  i.e.,  its  boundary  is  entirely  made  of  line  segments.3  As  the 
boundary  of  Cfree  coincides  with  the  boundary  of  CO ,  this  assumption  implies 
that  the  C-obstacle  region  is  itself  a  polygonal  subset  of  C. 

For  each  configuration  q  in  Cfree,  let  its  clearance  be  defined  as 

7(9)  =o  min  \\q  —  s||,  (12.6) 

S£.C/L-  free 

where  dCfree  is  the  boundary  of  Cfree-  The  clearance  7 (<7)  represents  the  min¬ 
imum  Euclidean  distance  between  the  configuration  q  and  the  C-obstacle  re- 

2  Many  algorithms  based  on  computational  geometry  techniques  are  available  (in 
the  literature,  but  also  implemented  in  software  packages)  to  test  collision  in 
IR2  o  IR3.  The  most  efficient,  such  as  I-Collide  and  V-Collide,  use  a  hierarchical 
representation  of  geometric  models  of  bodies,  and  can  speed  up  collision  checking 
by  re-using  the  results  of  previous  checks  in  spatially  similar  situations. 

3  According  to  the  definition,  a  polygonal  subset  is  not  necessarily  connected,  and 
may  contain  ‘holes’. 
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Fig.  12.5.  An  example  of  generalized  Voronoi  diagram  and  the  solution  of  a  par¬ 
ticular  instance  of  the  planning  problem,  obtained  by  retracting  qs  and  qg  on  the 
diagram.  The  solution  path  from  qs  to  qg  consists  of  the  two  dashed  segments  and 
the  thick  portion  of  the  diagram  joining  them 


gion.  Moreover,  consider  the  set  of  points  on  the  boundary  of  Cfree  that  are 
neighbours  of  q: 


N(q)  =  {s  G  dCbee  :  \\q  -  s||  =  7 (<?)},  (12.7) 

i.e.,  the  points  on  dCflee  that  determine  the  value  of  the  clearance  for  q.  With 
these  definitions,  the  generalized 4  Voronoi  diagram  of  Cfree  is  the  locus  of  its 
configurations  having  more  than  one  neighbour: 

V(Cfree)  ={ge  Cfree  :  card(lV(<7))  >  1},  (12.8) 

in  which  card(-)  denotes  the  cardinality  of  a  set.  Figure  12.5  shows  an  example 
of  generalized  Voronoi  diagram  for  a  polygonal  free  configuration  space;  note 
how  the  connectivity  of  Cfree  is  well  captured  by  the  diagram. 

It  is  easy  to  show  that  V(Cfree)  is  made  of  elementary  arcs  that  are  either 
rectilinear  —  each  made  of  contiguous  configurations  whose  clearance  is  due  to 
the  same  pair  of  edges  or  vertices  —  or  parabolic  —  each  made  of  contiguous 
configurations  whose  clearance  is  determined  by  the  same  pair  edge-vertex. 
Therefore,  one  can  build  an  analytical  expression  of  V(Cfree)  starting  from 
the  pair  of  features  (side/side,  side/vertex,  vertex/ vertex)  that  determine  the 

4  A  proper  Voronoi  diagram  is  obtained  in  the  particular  case  in  which  the  C- 
obstacles  are  isolated  points. 
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Fig.  12.6.  The  retraction  procedure  for  connecting  a  generic  configuration  q  in  Cfree 
to  V (Cfree) 


appearance  of  each  arc.  From  an  abstract  point  of  view,  V(Cfree)  can  be  consid¬ 
ered  as  a  graph  having  elementary  arcs  of  the  diagram  as  arcs  and  endpoints 
of  arcs  as  nodes. 

By  construction,  the  generalized  Voronoi  diagram  has  the  property  of  lo¬ 
cally  maximizing  clearance,  and  is  therefore  a  natural  choice  as  a  roadmap 
of  Cfree  for  planning  motions  characterized  by  a  healthy  safety  margin  with 
respect  to  the  possibility  of  collisions.  To  use  V(Cfree)  as  a  roadmap,  a  retrac¬ 
tion  procedure  must  be  defined  for  connecting  a  generic  configuration  in  Cfree 
to  the  diagram.  To  this  end,  consider  the  geometric  construction  shown  in 
Fig.  12.6.  Since  q  ^  V(Cfree),  it  is  card(7V(g))  =  1,  i.e.,  there  exists  a  single 
point  on  the  polygonal  boundary  of  CO  (either  a  vertex  or  a  point  on  a  side) 
that  determines  the  value  of  the  clearance  7(9).  The  gradient  V7 (q),  which 
identifies  the  direction  of  steepest  ascent  for  the  clearance  at  the  configuration 
q,  is  directed  as  the  half-line  originating  in  N(q)  and  passing  through  q.  The 
first  intersection  of  this  half-line  with  V(Cfree)  defines  r(q),  i.e.,  the  connec¬ 
tion  point  of  q  to  the  generalized  Voronoi  diagram.  To  guarantee  that  r(-)  is 
continuous,  it  is  convenient  to  extend  its  domain  of  definition  to  all  Cfree  by 
letting  r(q)  =  q  if  q  <E  V(Cfree)- 

From  a  topological  viewpoint,  the  function  r(-)  defined  above  is  actually 
an  example  of  retraction  of  Cfree  on  V(Cfree),  i.e.,  a  continuous  surjective  map 
from  Cfree  to  V(Cfree)  such  that  its  restriction  to  V(Cfree)  is  the  identity  map. 
In  view  of  its  definition,  r(-)  preserves  the  connectivity  of  Cfree,  in  the  sense 
that  q  and  r(q)  —  as  well  as  the  segment  joining  them  —  always  lie  in  the 
same  connected  component  of  Cfree-  This  property  is  particularly  important, 
because  it  is  possible  to  show  that,  given  a  generic  connectivity-preserving 
retraction  p  of  Cfree  on  a  roadmap  1Z,  there  exists  a  free  path  between  two 
configurations  qs  and  qg  if  and  only  if  there  exists  a  path  on  1Z  between  p(qs) 
and  p(qg).  As  a  consequence,  the  problem  of  planning  a  path  in  Cfree  reduces 
to  the  problem  of  planning  a  path  on  its  retraction  1Z  =  p(Cfree). 
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Given  the  start  and  goal  configurations  qs  and  qg,  the  motion  planning 
method  via  retraction  goes  through  the  following  steps  (see  Fig.  12.5): 

1.  Build  the  generalized  Voronoi  diagram  V(Cfree)- 

2.  Compute  the  retractions  r(qa)  and  r(qg)  on  V(Cfree)- 

3.  Search  V(Cfree)  for  a  sequence  of  consecutive  arcs  such  that  r(qs)  belongs 
to  the  first  and  r(q  )  to  the  last. 

4.  If  the  search  is  successful,  replace  the  first  arc  of  the  sequence  with  its 
subarc  originating  in  r(qs)  and  the  last  arc  of  the  sequence  with  its  subarc 
terminating  in  r{qg),  and  provide  as  output  the  path  consisting  of  the 
line  segment  joining  qs  to  r(qs),  the  modified  arc  sequence,  and  the  line 
segment  joining  qg  to  r(qg)-  otherwise,  report  a  failure. 

If  simplicity  of  implementation  is  desired,  the  graph  search5  required  at 
Step  3  can  be  performed  using  basic  strategies  such  as  breadth-first  or  depth- 
first  search.  On  the  other  hand,  if  one  wishes  to  identify  the  minimum-length 
path  (among  those  which  can  be  produced  by  the  method)  between  qs  and 
qg,  each  arc  must  be  labelled  with  a  cost  equal  to  its  actual  length.  The 
minimum-cost  path  can  then  be  computed  with  an  informed  algorithm 
i.e. ,  an  algorithm  using  a  heuristic  estimate  of  the  minimum  cost  path  from  a 
generic  node  to  the  goal,  in  this  case  the  Euclidean  distance  —  such  as  A*. 

Whatever  the  adopted  search  strategy,  the  above  motion  planning  method 
via  retraction  of  Cfree  on  V(Cfree)  is  complete,  i.e.,  it  is  guaranteed  to  find  a 
solution  if  one  exists,  and  to  report  a  failure  otherwise.  Its  time  complexity  is  a 
function  of  the  number  v  of  vertices  of  the  polygonal  region  Cfree,  and  depends 
essentially  on  the  construction  of  the  generalized  Voronoi  diagram  (Step  1), 
on  the  retraction  of  qs  and  qg  (Step  2),  and  on  the  search  on  the  diagram 
(Step  3).  As  for  Step  1,  the  most  efficient  algorithms  can  build  V(Cfree)  in  time 
O(rTogri).  The  retraction  procedure  requires  0(v),  mainly  to  compute  N(qs) 
and  N(q  ).  Finally,  since  it  is  possible  to  prove  that  V(Cfree)  has  0(v)  arcs, 
the  complexity  of  breadth- first  or  depth- first  search  would  be  0(v),  whereas 
A *  would  require  O(vlogu).  Altogether,  the  time  complexity  of  the  motion 
planning  method  via  retraction  is  O(ulogu). 

It  should  be  noted  that,  once  the  generalized  Voronoi  diagram  of  Cfree 
has  been  computed,  it  can  be  used  again  to  solve  quickly  other  instances 
( queries )  of  the  same  motion  planning  problem,  i.e.,  to  generate  collision-free 
paths  between  different  start  and  goal  configurations  in  the  same  Cflee-  For 
example,  this  is  useful  when  a  robot  must  repeatedly  move  between  different 
postures  in  the  same  static  workspace.  The  motion  planning  method  based  on 
retraction  can  then  be  considered  multiple- query .  It  is  also  possible  to  extend 
the  method  to  the  case  in  which  the  C-obstacles  are  generalized  polygons. 


5  See  Appendix  E  for  a  quick  survey  on  graph  search  strategies  and  algorithm 
complexity. 
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12.4  Planning  via  Cell  Decomposition 

Assume  that  the  free  configuration  space  Cfree  can  be  decomposed  in  simply- 
shaped  regions,  called  cells,  with  the  following  basic  characteristics: 

•  Given  two  configurations  belonging  to  the  same  cell,  it  is  ‘easy’  to  compute 
a  collision-free  path  that  joins  them. 

•  Given  two  adjacent  cells  —  i.e. ,  two  cells  having  in  common  a  portion  of 
their  boundaries  of  non-zero  measure  —  it  is  ‘easy’  to  generate  a  collision- 
free  path  going  from  one  cell  to  the  other. 

Starting  from  one  such  cell  decomposition  of  Cfree,  it  is  easy  to  build  the 
associated  connectivity  graph.  The  nodes  of  this  graph  represent  cells,  while  an 
arc  between  two  nodes  indicates  that  the  two  corresponding  cells  are  adjacent. 
By  searching  the  connectivity  graph  for  a  path  from  the  cell  containing  the 
start  configuration  qs  to  the  cell  containing  the  goal  configuration  qg,  one 
obtains  (if  it  exists)  a  sequence  of  adjacent  cells,  called  channel,  from  which 
it  is  possible  —  in  view  of  the  above  mentioned  characteristics  of  cells  —  to 
extract  a  path  that  joins  qs  to  qg  and  is  entirely  contained  in  Cfree. 

The  general  approach  so  far  outlined  generates  different  motion  planning 
methods,  essentially  depending  on  the  type  of  cells  used  for  the  decomposition. 
In  the  following,  two  algorithms  are  described,  respectively  based  on  exact  and 
approximate  cell  decomposition  of  Cfree-  As  before,  it  is  assumed  that  Cfree  is 
a  limited  polygonal  subset  of  C  =  1R2. 

12.4.1  Exact  Decomposition 

When  an  exact  decomposition  is  used,  the  free  configuration  space  is  parti¬ 
tioned  in  a  collection  of  cells  whose  union  gives  exactly  Cflee-  A  typical  choice 
for  cells  are  convex  polygons.  In  fact,  convexity  guarantees  that  the  line  seg¬ 
ments  joining  two  configurations  belonging  to  the  same  cell  lies  entirely  in 
the  cell  itself,  and  therefore  in  Cfree.  Moreover,  it  is  easy  to  travel  safely  be¬ 
tween  two  adjacent  cells  by  passing  through  the  midpoint  of  the  segment  that 
constitutes  their  common  boundary.  A  simple  way  to  decompose  Cfree  in  a 
collection  of  convex  polygons  is  to  use  the  sweep  line  algorithm,  which  turns 
out  to  be  useful  in  a  number  of  computational  geometry  problems.  In  the 
present  setting,  the  application  of  this  algorithm  proceeds  as  follows. 

Choose  a  line  that  is  not  parallel  to  any  side  of  the  boundary  of  Cfree,  and 
let  it  translate  (‘sweep’)  all  over  Cfree-  Whenever  the  line  passes  through  one 
of  the  vertices  of  Cfree,  consider  the  two  segments  ( extensions )  that  originate 
from  the  vertex,  lie  on  the  line  and  point  in  opposite  directions,  terminating 
at  the  first  intersection  with  dCfree.  Every  extension  that  lies  in  Cfree  (except 
for  its  endpoints)  is  part  of  the  boundary  of  a  cell;  the  rest  of  the  boundary  is 
made  of  (parts  of)  sides  of  dCfree  and  possibly  other  extensions.  This  procedure 
is  illustrated  in  Fig.  12.7,  where  a  vertical  sweep  line  has  been  used;  note  that 
some  of  the  vertices  of  Cfree  contribute  to  the  decomposition  with  a  single  or  no 
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Fig.  12.7.  An  example  of  trapezoidal  decomposition  via  the  sweep  line  algorithm 
(above)  and  the  associated  connectivity  graph  (below).  Also  shown  is  the  solution 
of  a  particular  planning  problem  (cs  =  C3  and  cg  =  C20),  both  as  a  channel  in  Cfree 
and  as  a  path  on  the  graph 


extension  at  all.  The  result  is  a  special  case  of  convex  polygonal  decomposition, 
that  is  called  trapezoidal  because  its  cells  are  trapezoids  —  triangular  cells,  if 
present,  are  regarded  as  degenerate  trapezoids  having  one  side  of  zero  length. 

After  the  decomposition  of  Cfree  has  been  computed,  it  is  possible  to  build 
the  associated  connectivity  graph  C.  This  is  the  graph  whose  nodes  are  the 
cells  of  the  decomposition,  while  an  arc  exists  between  two  nodes  if  the  cor¬ 
responding  cells  are  adjacent,  i.e. ,  the  intersection  of  their  boundary  is  a  line 
segment  of  non-zero  length;  therefore,  cells  with  side-vertex  or  vertex-vertex 
contacts  are  not  adjacent.  At  this  point,  it  is  necessary  to  identify  the  cells  cs 
and  cg  in  the  decomposition  that  respectively  contain  qs  and  qg,  the  start  and 
goal  configurations  for  the  considered  planning  problem.  A  graph  search  algo¬ 
rithm  can  then  be  used  to  find  a  channel  from  cs  to  cg,  i.e.,  a  path  on  C  that 
joins  the  two  corresponding  nodes  (see  Fig.  12.7).  From  the  channel,  which  is 
a  sequence  of  adjacent  cells,  one  must  extract  a  path  in  Cflee  going  from  qs 
to  qg.  Since  the  interior  of  the  channel  is  contained  in  Cfree  and  the  cells  are 
convex  polygons,  such  extraction  is  straightforward.  For  example,  one  may 
identify  the  midpoints  of  the  segments  that  represent  the  common  boundaries 
between  consecutive  cells  of  the  channel,  and  connect  them  through  a  broken 
line  starting  in  qs  and  ending  in  qg. 
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Wrapping  up,  given  the  two  configurations  qs  and  qg.  the  motion  planning 
algorithm  via  exact  cell  decomposition  is  based  on  the  following  steps: 

1.  Compute  a  convex  polygonal  (e.g.,  trapezoidal)  decomposition  of  Cfree- 

2.  Build  the  associated  connectivity  graph  C . 

3.  Search  C  for  a  channel,  i.e.,  a  sequence  of  adjacent  cells  from  cs  to  cg. 

4.  If  a  channel  has  been  found,  extract  and  provide  as  output  a  collision-free 
path  from  qs  to  qg\  otherwise,  report  a  failure. 

As  in  motion  planning  via  retraction,  using  a  non-informed  graph  search 
algorithm  in  Step  3  will  result  in  a  channel  that  is  not  optimal,  in  the  sense 
that  all  paths  from  qs  to  qg  that  can  be  extracted  from  the  channel  may  be 
longer  than  necessary.  To  compute  efficient  paths,  the  use  of  A*  is  advisable 
as  a  search  algorithm.  To  this  end,  one  should  build  a  modified  connectivity 
graph  C'  having  as  nodes  qs,  qg  and  all  the  midpoints  of  adjacency  segments 
between  cells,  and  arcs  joining  nodes  belonging  to  the  same  cell  (note  that 
nodes  on  adjacency  segments  belong  to  two  cells).  Each  arc  is  then  labelled 
with  a  cost  equal  to  the  distance  between  the  nodes  connected  by  the  arc.  If 
the  heuristic  function  is  chosen  as  the  distance  between  the  current  node  and 
qg,  the  use  of  A*  will  produce  the  shortest  path  in  C' ,  if  a  solution  exists. 

The  motion  planning  method  based  on  exact  cell  decomposition  is  com¬ 
plete.  As  for  its  time  complexity,  it  depends  mainly  on  the  cell  decomposition 
and  on  the  connectivity  graph  search.  Using  the  sweep  line  algorithm,  the 
decomposition  procedure  (including  the  generation  of  the  connectivity  graph) 
has  complexity  O(ulogu),  where  v  is  the  number  of  vertices  of  Cfree-  More¬ 
over,  it  can  be  shown  that  the  connectivity  graph  C  has  0{v)  arcs.  Hence, 
regardless  of  the  adopted  search  strategy,  the  motion  planning  method  based 
on  exact  cell  decomposition  has  complexity  0(v  log  v). 

Note  the  following  facts: 

•  Any  planner  based  on  exact  cell  decomposition  can  be  considered  multiple- 
query.  In  fact,  once  computed,  the  connectivity  graph  associated  with  the 
decomposition  can  be  used  to  solve  different  instances  of  the  same  motion 
planning  problem. 

•  The  connectivity  graph  represents  a  network  of  channels,  each  implicitly 
containing  an  infinity  of  paths  that  traverse  the  channel  and  are  topolog¬ 
ically  equivalent,  i.e.,  differ  only  for  a  continuous  deformation.  Therefore, 
cell  decomposition  provides  as  output  a  structure  that  is  more  flexible 
than  the  roadmap  used  by  retraction-based  methods.  This  may  be  useful 
to  plan  paths  in  the  channel  that  are  also  admissible  with  respect  to  possi¬ 
ble  kinematic  constraints,  as  well  as  to  avoid  unexpected  obstacles  during 
the  actual  execution  of  the  motion. 

•  The  solution  paths  produced  by  the  planning  method  based  on  exact  cell 
decomposition  are  broken  lines.  It  is  however  possible  to  smooth  the  path 
using  curve  fitting  techniques.  In  practice,  one  selects  a  sufficient  number 
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of  intermediate  points  ( via  points)  on  the  path,  among  which  it  is  nec¬ 
essary  to  include  qs  and  qgl  and  then  interpolates  them  using  functions 
with  an  appropriate  level  of  differentiability  (e.g.,  polynomial  functions  of 
sufficiently  high  degree). 

•  The  above  method  can  be  extended  to  the  case  in  which  Cfree  is  a  limited 
polyhedral  subset  of  C  =  IR3 .  In  particular,  the  decomposition  of  Cflee  can 
obtained  through  the  sweep  plane  algorithm,  which  produces  polyhedral 
cells.  The  common  boundary  between  adjacent  cells  consists  of  trapezoid  of 
non-zero  area.  This  boundary  can  be  safely  crossed,  e.g.,  at  the  barycentre 
of  the  trapezoid. 

Finally,  it  should  be  mentioned  that  there  exist  in  the  literature  methods 
based  on  exact  cell  decomposition  that  can  solve  essentially  any  motion  plan¬ 
ning  problem,  regardless  of  the  dimension  of  C  and  of  the  geometry  of  Cfree, 
which  can  also  be  non-polyhedral.  However,  the  complexity  of  these  planners 
is  prohibitive,  being  exponential  in  the  dimension  of  C,  and  their  importance 
is  therefore  mainly  theoretical. 


12.4.2  Approximate  Decomposition 

In  approximate  decompositions  of  Cfree,  disjoint  cells  of  predefined  shape  are 
used;  for  example,  when  C  =  IR2  one  may  choose  square  or  rectangular  cells.  In 
general,  the  union  of  all  cells  will  represent  an  approximation  by  defect  of  Cfree- 
To  achieve  a  reasonable  trade-off  between  the  accuracy  of  the  approximation 
and  the  efficiency  of  the  decomposition  procedure,  a  recursive  algorithm  is 
typically  used,  which  starts  with  a  coarse  grid  whose  resolution  is  locally 
increased  to  adapt  better  to  the  geometry  of  Cfree-  As  in  motion  planning 
methods  based  on  exact  decomposition,  the  connectivity  graph  associated 
with  the  obtained  approximate  decomposition  is  searched  for  a  channel,  from 
which  a  solution  path  can  be  extracted. 

In  the  following,  a  motion  planning  method  based  on  approximate  decom¬ 
position  is  described  for  the  case  in  which  Cfree  is  a  limited  polygonal  subset 
of  C  =  IR2.  Without  loss  of  generality,  it  is  assumed  that  the  ‘external’  bound¬ 
ary  of  Cfree  is  a  square,  and  square  cells  are  therefore  used.  The  decomposition 
algorithm  (Fig.  12.8)  starts  by  dividing  initially  the  square  containing  Cfree 
into  four  cells,  that  are  classified  according  to  the  categories  below: 

•  free  cells,  whose  interior  has  no  intersection  with  the  C-obstacle  region; 

•  occupied  cells,  entirely  contained  in  the  C-obstacle  region; 

•  mixed  cells,  that  are  neither  free  nor  occupied. 

At  this  point,  one  builds  the  connectivity  graph  C  associated  with  the 
current  level  of  decomposition:  this  is  the  graph  having  free  and  mixed  cells 
as  nodes,  and  arcs  that  join  nodes  representing  adjacent  cells.  Once  the  nodes 
corresponding  to  the  cells  that  contain  qs  and  qg  have  been  identified,  C  is 
searched  for  a  path  between  them,  e.g.,  using  the  A*  algorithm.  If  such  a  path 
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Fig.  12.8.  An  example  of  motion  planning  via  approximate  cell  decomposition;  left-. 
the  assigned  problem,  right:  the  solution  as  a  free  channel  ( thick  line) 


does  not  exist,  a  failure  is  reported.  If  the  path  exists,  it  consists  of  a  sequence 
of  cells  that  may  be  either  all  free  ( free  channel)  or  not  ( mixed  channel).  In 
the  first  case,  a  solution  to  the  motion  planning  problem  has  been  found;  in 
particular,  a  configuration  space  path  can  be  easily  extracted  from  the  free 
channel  as  in  the  method  based  on  exact  cell  decomposition.  Instead,  if  the 
channel  contains  mixed  cells,  each  of  them  is  further  divided  into  four  cells, 
which  are  then  classified  as  free,  occupied  or  mixed.  The  algorithm  proceeds 
by  iterating  these  steps,  until  a  free  channel  going  from  qs  to  qg  has  been 
found  or  a  minimum  admissible  size  has  been  reached  for  the  cells.  Figure  12.8 
shows  an  example  of  application  of  this  technique.  Note  that,  at  the  resolution 
level  where  the  solution  has  been  found,  free  and  occupied  cells  represent  an 
approximation  by  defect  of  Cfree  and  CO ,  respectively.  The  missing  part  of  C 
is  occupied  by  mixed  cells  (in  gray). 

At  each  iteration,  the  search  algorithm  on  the  connectivity  graph  can  find 
a  free  channel  going  from  qs  to  qg  only  if  such  a  channel  exists  on  the  approx¬ 
imation  of  Cfree  (he.,  on  the  free  cells)  at  the  current  level  of  decomposition. 
This  motion  planning  method  is  therefore  resolution  complete ,  in  the  sense 
that  a  sufficient  reduction  of  the  minimum  admissible  size  for  cells  guarantees 
that  a  solution  is  found  whenever  one  exists. 

A  comparison  between  Figs.  12.7  and  12.8  clearly  shows  that,  unlike  what 
happens  in  exact  decomposition,  the  boundary  of  a  cell  in  an  approximate 
decomposition  does  not  correspond  in  general  to  a  change  in  the  spatial  con¬ 
straints  imposed  by  obstacles.  One  of  the  consequences  of  this  fact  is  that 
the  implementation  of  a  motion  planning  method  based  on  approximate  de¬ 
composition  is  remarkably  simpler,  as  it  only  requires  a  recursive  division  of 
cells  followed  by  a  collision  check  between  the  cells  and  the  C-obstacle  region. 
In  particular,  the  first  can  be  realized  using  a  data  structure  called  quadtree. 
This  is  a  tree  in  which  any  internal  node  (i.e.,  a  node  that  is  not  a  leaf)  has 
exactly  four  child  nodes.  In  the  cell  decomposition  case,  the  tree  is  rooted  at 
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C,  i.e.,  the  whole  configuration  space.  Lower  level  nodes  represent  cells  that 
are  free,  occupied  or  mixed;  only  the  latter  have  children. 

Another  difference  with  respect  to  the  method  based  on  exact  decompo¬ 
sition  is  that  an  approximate  decomposition  is  intrinsically  associated  with 
a  particular  instance  of  a  planning  problem,  as  the  decomposition  procedure 
itself  is  guided  by  the  search  for  a  free  channel  between  start  and  goal. 

The  planning  method  based  on  approximate  decomposition  is  conceptually 
applicable  in  configuration  spaces  of  arbitrary  dimension.  For  example,  in 
1R3  it  is  possible  to  use  an  octree,  a  tree  in  which  any  internal  node  has 
eight  children.  In  IR”,  the  corresponding  data  structure  is  a  2 n-tree.  Since  the 
maximum  number  of  leaves  of  a  2n-tree  is  2np,  where  p  is  the  depth  (number 
of  levels)  of  the  tree,  the  complexity  of  approximate  cell  decomposition  - 
and  thus  of  the  associated  motion  planning  method  —  is  exponential  in  the 
dimension  of  C  and  in  the  maximal  resolution  of  the  decomposition.  As  a 
consequence,  this  technique  is  effective  in  practice  only  in  configuration  spaces 
of  low  dimension  (typically,  not  larger  than  4) . 


12.5  Probabilistic  Planning 

Probabilistic  planners  represent  a  class  of  methods  of  remarkable  efficiency, 
especially  in  problems  involving  high-dimensional  configuration  spaces.  They 
belong  to  the  general  family  of  sampling-based  methods,  whose  basic  idea  con¬ 
sists  of  determining  a  finite  set  of  collision-free  configurations  that  adequately 
represent  the  connectivity  of  Cfree,  and  using  these  configurations  to  build  a 
roadmap  that  can  be  employed  for  solving  motion  planning  problems.  This 
is  realized  by  choosing  at  each  iteration  a  sample  configuration  and  checking 
if  it  entails  a  collision  between  the  robot  and  the  workspace  obstacles.  If  the 
answer  is  affirmative,  the  sample  is  discarded.  A  configuration  that  does  not 
cause  a  collision  is  instead  added  to  the  current  roadmap  and  connected  if 
possible  to  other  already  stored  configurations. 

The  above  strategy  is  quite  general  and  may  lead  to  different  planning 
methods  depending  on  the  specific  design  choices,  and  mainly  on  the  criterion 
for  selecting  the  samples  in  C  to  be  checked  for  collision.  One  may  proceed 
in  a  deterministic  fashion,  choosing  the  samples  by  means  of  a  regular  grid 
that  is  applied  to  C.  However,  it  is  preferable  to  use  a  randomized  approach, 
in  which  the  sample  configurations  are  chosen  according  to  some  probability 
distribution.  In  the  following,  two  planners  of  this  type  are  described. 

12.5.1  PRM  Method 

The  basic  iteration  of  the  PRM  ( Probabilistic  Roadmap)  method  begins  by 
generating  a  random  sample  <7rand  of  the  configuration  space  using  a  uniform 
probability  distribution  in  C.  Then,  qrand  is  tested  for  collision,  by  using  kine¬ 
matic  and  geometric  relationships  to  compute  the  corresponding  posture  of 
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Fig.  12.9.  A  PRM  in  a  two-dimensional  configuration  space  (left)  and  its  use  for 
solving  a  particular  planning  problem  (right) 


the  robot  and  invoking  an  algorithm  that  can  detect  collisions  (including  con¬ 
tacts)  between  the  latter  and  the  obstacles.  If  qrand  does  not  cause  collisions, 
it  is  added  to  the  roadmap  and  connected  (if  possible)  through  free  local  paths 
to  sufficiently  ‘near’  configurations  already  in  the  roadmap.  Usually,  ‘near¬ 
ness’  is  defined  on  the  basis  of  the  Euclidean  distance  in  C,  but  it  is  possible 
to  use  different  distance  notions;  for  example,  as  mentioned  in  Sect.  12.2.1, 
one  may  use  a  configuration  space  distance  notion  induced  by  a  distance  in 
the  workspace.  The  generation  of  a  free  local  path  between  qrand  and  a  near 
configuration  qnear  is  delegated  to  a  procedure  known  as  local  planner.  A  com¬ 
mon  choice  is  to  throw  a  rectilinear  path  in  C  between  qrand  and  qnear  and 
test  it  for  collision,  for  example  by  sampling  the  segment  with  sufficient  res¬ 
olution  and  checking  the  single  samples  for  collision.  If  the  local  path  causes 
a  collision,  it  is  discarded  and  no  direct  connection  between  qrand  and  qnear 
appears  in  the  roadmap. 

The  PRM  incremental  generation  procedure  stops  when  either  a  maximum 
number  of  iterations  has  been  reached,  or  the  number  of  connected  compo¬ 
nents  in  the  roadmap  becomes  smaller  than  a  given  threshold.  At  this  point, 
one  verifies  whether  it  is  possible  to  solve  the  assigned  motion  planning  prob¬ 
lem  by  connecting  qs  and  qg  to  the  same  connected  component  of  the  PRM 
by  free  local  paths.  Figure  12.9  shows  an  example  of  PRM  and  the  solution 
to  a  particular  problem.  Note  the  presence  of  multiple  connected  components 
of  the  PRM,  one  of  which  consists  of  a  single  configuration. 

If  a  solution  cannot  be  found,  the  PRM  can  be  improved  by  performing 
more  iterations  of  the  basic  procedure,  or  using  special  strategies  aimed  at 
reducing  the  number  of  its  connected  components.  For  example,  a  possible 
technique  consists  of  trying  to  connect  configurations  that  are  close  but  belong 
to  different  components  via  more  general  (e.g.,  not  rectilinear)  local  paths. 

The  main  advantage  of  the  PRM  method  is  its  remarkable  speed  in  finding 
a  solution  to  motion  planning  problems,  provided  that  the  roadmap  has  been 
sufficiently  developed.  In  this  respect,  it  should  be  noted  that  new  instances 
of  the  same  problem  induce  a  potential  enhancement  of  the  PRM,  which 
improves  with  usage  both  in  terms  of  connectivity  and  of  time  efficiency.  The 
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PRM  method  is  therefore  intrinsically  multiple-query.  In  high-dimensional 
configuration  spaces,  the  time  needed  by  this  method  to  compute  a  solution 
can  be  several  orders  of  magnitude  smaller  than  with  the  previously  presented 
techniques.  Another  aspect  of  the  method  that  is  worth  mentioning  is  the 
simplicity  of  implementation.  In  particular,  note  that  the  generation  of  C- 
obstacles  is  completely  eliminated. 

The  downside  of  the  PRM  method  is  that  it  is  only  probabilistically  com¬ 
plete ,  i.e.,  the  probability  of  finding  a  solution  to  the  planning  problem  when 
one  exists  tends  to  1  as  the  execution  time  tends  to  infinity.  This  means  that,  if 
no  solution  exists,  the  algorithm  will  run  indefinitely.  In  practice,  a  maximum 
number  of  iterations  is  enforced  so  as  to  guarantee  its  termination. 

A  situation  that  is  critical  for  the  PRM  method  is  the  presence  of  narrow 
passages  in  Cflee,  such  as  the  one  shown  in  the  upper-right  quadrant  of  the 
scene  in  Fig.  12.9.  In  fact,  using  a  uniform  distribution  for  generating  qrand,  the 
probability  of  placing  a  sample  in  a  certain  region  of  Cfree  is  proportional  to  its 
volume.  As  a  consequence,  depending  on  its  size,  it  may  be  very  unlikely  that  a 
path  crossing  a  narrow  passage  appears  in  the  PRM  within  a  reasonable  time. 
To  alleviate  this  problem,  the  method  can  be  modified  by  using  non-uniform 
probability  distributions.  For  example,  there  exist  strategies  for  generating 
9rand  that  are  biased  towards  those  regions  of  C  that  contain  fewer  samples, 
and  therefore  are  more  likely  to  contain  close  obstacles  and  the  associated 
narrow  passages. 

12.5.2  Bidirectional  RRT  Method 

Single-query  probabilistic  methods  are  aimed  at  quickly  solving  a  particular 
instance  of  a  motion  planning  problem.  Unlike  multiple-query  planners  such 
as  PRM,  these  techniques  do  not  rely  on  the  generation  of  a  roadmap  that 
represents  exhaustively  the  connectivity  of  the  free  configuration  space;  in 
fact,  they  tend  to  explore  only  a  subset  of  Cfree  that  is  relevant  for  solving  the 
problem  at  hand.  This  results  in  a  further  reduction  of  the  time  needed  to 
compute  a  solution. 

An  example  of  single-query  probabilistic  planner  is  the  bidirectional  RRT 
method,  which  makes  use  of  a  data  structure  called  RRT  {Rapidly- exploring 
Random  Tree).  The  incremental  expansion  of  an  RRT,  denoted  by  T  in  the 
following,  relies  on  a  simple  randomized  procedure  to  be  repeated  at  each 
iteration  (see  Fig.  12.10).  The  first  step  is  the  generation  of  a  random  config¬ 
uration  qrand  according  to  a  uniform  probability  distribution  in  C  (as  in  the 
PRM  method).  Then,  the  configuration  qnear  in  T  that  is  closer  to  qrand  is 
found,  and  a  new  candidate  configuration  qnew  is  produced  on  the  segment 
joining  qneal.  to  qrand  at  a  predefined  distance  <5  from  qnear.  A  collision  check 
is  then  run  to  verify  that  both  qnew  and  the  segment  going  from  qnear  to  qnew 
belong  to  Cfree-  If  this  is  the  case,  T  is  expanded  by  incorporating  qnew  and 
the  segment  joining  it  to  qnear.  Note  that  qrand  is  not  added  to  the  tree,  so 
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Fig.  12.10.  The  bidirectional  RRT  method  in  a  two-dimensional  configuration  space 
left:  the  randomized  mechanism  for  expanding  a  tree,  right:  the  extension  procedure 
for  connecting  the  two  trees 


that  it  is  not  necessary  to  check  whether  it  belongs  to  Cfree;  its  only  function 
is  to  indicate  a  direction  of  expansion  for  T. 

It  is  worth  pointing  out  that  the  RRT  expansion  procedure,  although  quite 
simple,  results  in  a  very  efficient  ‘exploration’  of  C.  In  fact,  it  may  be  shown 
that  the  procedure  for  generating  new  candidate  configuration  is  intrinsically 
biased  towards  those  regions  of  Cfree  that  have  not  been  visited  yet.  Moreover, 
the  probability  that  a  generic  configuration  of  Cfree  is  added  to  the  RRT  tends 
to  1  as  the  execution  time  tends  to  infinity,  provided  that  the  configuration 
lies  in  the  same  connected  component  of  Cfree  where  the  RRT  is  rooted. 

To  speed  up  the  search  for  a  free  path  going  from  qs  to  qg,  the  bidirec¬ 
tional  RRT  method  uses  two  trees  Ts  and  Tg,  respectively  rooted  at  qs  and 
qg.  At  each  iteration,  both  trees  are  expanded  with  the  previously  described 
randomized  mechanism.  After  a  certain  number  of  expansion  steps,  the  algo¬ 
rithm  enters  a  phase  where  it  tries  to  connect  the  two  trees  by  extending  each 
one  of  them  towards  the  other.  This  is  realized  by  generating  a  qnew  as  an  ex¬ 
pansion  of  Ts,  and  trying  to  connect  Tg  to  qnew.  To  this  end,  one  may  modify 
the  above  expansion  procedure.  In  particular,  once  qnew  has  been  generated 
from  Ts,  it  acts  as  a  qrand  for  Tg:  one  finds  the  closest  configuration  qnear  in 
Tg,  and  moves  from  qneal.  trying  to  actually  reach  <7rand  =  qnew,  hence  with  a 
variable  stepsize  as  opposed  to  a  constant  6. 

If  the  segment  joining  qnear  to  qnew  is  collision-free,  the  extension  is  com¬ 
plete  and  the  two  trees  have  been  connected;  otherwise  the  free  portion  of  the 
segment  is  extracted  and  added  to  Tg  together  with  its  endpoint.  At  this  point, 
Tg  and  Ts  exchange  their  roles  and  the  connection  attempt  is  repeated.  If  this 
is  not  successful  within  a  certain  number  of  iterations,  one  may  conclude  that 
the  two  trees  are  still  far  apart  and  resume  the  expansion  phase. 

Like  the  PRM  method,  bidirectional  RRT  is  probabilistically  complete.  A 
number  of  variations  can  be  made  on  the  basic  scheme.  For  example,  rather 
than  using  a  constant  S  for  generating  qnew,  one  may  define  the  stepsize  as  a 
function  of  the  available  free  space,  possibly  going  as  far  as  qrand  (as  in  the 
extension  procedure).  This  greedy  version  of  the  method  can  be  much  more 
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efficient  if  C  contains  extensive  free  regions.  Moreover,  RRT-based  methods 
can  be  adapted  to  robots  that  do  not  satisfy  the  free-flying  assumption,  such 
as  robots  that  are  subject  to  nonholonomic  constraints. 

Extension  to  nonholonomic  robots 

Consider  now  the  motion  planning  problem  for  a  nonholonomic  mobile  robot 
whose  kinematic  model  is  expressed  as  (11.10).  As  seen  in  the  previous  chap¬ 
ter,  admissible  paths  in  configuration  space  must  satisfy  constraint  (11.40). 
For  example,  in  the  case  of  a  robot  with  unicycle  kinematics,  rectilinear  paths 
in  configuration  space  —  such  as  those  used  in  the  RRT  expansion  to  move 
from  qnear  to  qnew  —  are  not  admissible  in  general. 

A  simple  yet  general  approach  to  the  design  of  nonholonomic  motion  plan¬ 
ning  methods  is  to  use  motion  primitives ,  i.e.,  a  finite  set  of  admissible  local 
paths  in  configuration  space,  each  produced  by  a  specific  choice  of  the  velocity 
inputs  in  the  kinematic  model.  Admissible  paths  are  generated  as  a  concate¬ 
nation  of  motion  primitives.  In  the  case  of  a  unicycle  robot,  for  example,  the 
following  set  of  velocity  inputs 

v  =  v  u>  =  {— u),0,u>}  te[0,A]  (12.9) 

results  in  three6  admissible  local  paths:  the  first  and  the  third  are  respectively 
a  left  turn  and  a  right  turn  along  arcs  of  circle,  while  the  second  is  a  rectilinear 
path  (Fig.  12.11,  left). 

The  expansion  of  an  RRT  for  a  nonholonomic  mobile  robot  equipped  with 
a  set  of  motion  primitives  is  quite  similar  to  the  previously  described  proce¬ 
dure.  The  difference  is  that,  once  identified  the  configuration  qnear  on  T  that 
is  closest  to  qrand,  the  new  configuration  qnew  is  generated  by  applying  the 
motion  primitives  starting  from  gnear  and  choosing  one  of  the  produced  con¬ 
figurations,  either  randomly  or  as  the  closest  to  qrand.  Clearly,  qnew  and  the 
admissible  local  path  joining  qnear  to  qnew  are  subject  to  a  collision  test.  Fig¬ 
ure  12.11,  right,  shows  an  example  of  RRT  —  more  precisely,  its  projection  on 
the  workspace  —  for  a  unicycle  equipped  with  the  motion  primitives  (12.9). 

Under  suitable  assumptions,  it  is  possible  to  show  that  if  the  goal  configu¬ 
ration  q  can  be  reached  from  the  start  configuration  qs  through  a  collision- 
free  concatenation  of  motion  primitives,7  the  probability  that  qg  is  added  to 

6  Note  that  these  particular  motion  primitives  include  neither  backward  motion 
nor  rotation  on  the  spot.  A  unicycle  with  constant  positive  driving  velocity  v  and 
bounded  steering  velocity  is  called  Dubins  car  in  the  literature. 

7  This  hypothesis,  obviously  necessary,  implies  that  the  choice  of  motion  primitives 
must  be  sufficiently  ‘rich’  to  guarantee  that  the  set  of  configuration  reachable 
through  concatenation  is  ‘dense’  enough  with  respect  to  Cfree-  For  example,  the 
unicycle  with  the  motion  primitives  (12.9)  with  a  variable  time  interval  A  can 
reach  any  configuration  in  Cfree,  although  not  necessarily  with  a  path  that  is 
entirely  contained  in  Cfree-  This  property  is  instead  guaranteed  if  the  Reeds-Shepp 
curves  given  by  (11.56)  are  used  as  motion  primitives. 
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Fig.  12.11.  RRT-based  motion  planning  for  a  unicycle;  left:  a  set  of  motion  primi¬ 
tives,  right:  an  example  of  RRT 


the  tree  T  tends  to  1  as  the  execution  time  tends  to  infinity.  To  increase  the 
efficiency  of  the  search,  also  in  this  case  it  is  possible  to  devise  a  bidirectional 
version  of  the  method. 


12.6  Planning  via  Artificial  Potentials 

All  the  methods  so  far  presented  are  suitable  for  off-line  motion  planning, 
because  they  require  a  priori  knowledge  of  the  geometry  and  the  pose  of  the 
obstacles  in  the  robot  workspace.  This  assumption  is  reasonable  in  many  cases, 
e.g.,  when  an  industrial  manipulator  is  moving  in  a  robotized  cell.  However,  in 
service  robotics  applications  the  robot  must  be  able  to  plan  its  motion  on-line, 
i.e.,  using  partial  information  on  the  workspace  gathered  during  the  motion 
on  the  basis  of  sensor  measurements. 

An  effective  paradigm  for  on-line  planning  relies  on  the  use  of  artificial 
potential  fields.  Essentially,  the  point  that  represents  the  robot  in  configura¬ 
tion  space  moves  under  the  influence  of  a  potential  field  U  obtained  as  the 
superposition  of  an  attractive  potential  to  the  goal  and  a  repulsive  potential 
from  the  C-obstacle  region.  Planning  takes  place  in  an  incremental  fashion: 
at  each  robot  configuration  q,  the  artificial  force  generated  by  the  potential 
is  defined  as  the  negative  gradient  —  Vf7(q)  of  the  potential,  which  indicates 
the  most  promising  direction  of  local  motion. 

12.6.1  Attractive  Potential 

The  attractive  potential  is  designed  so  as  to  guide  the  robot  to  the  goal  con¬ 
figuration  qg.  To  this  end,  one  may  use  a  paraboloid  with  vertex  in  qg: 

Uai{q)  =  ^  kaeT (q)e(q)  =  ^  fca||e(q)||2, 


(12.10) 
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Fig.  12.12.  The  shape  of  the  paraboloidal  attractive  potential  Uai  ( left )  and  of  the 
conical  attractive  potential  Ua 2  (right)  in  the  case  C  =  1R2,  for  ka  =  1 


where  ka  >  0  and  e  =  q  —  q  is  the  ‘error’  vector  with  respect  to  the  goal 
configuration  qg.  This  function  is  always  positive  and  has  a  global  minimum 
in  qg,  where  it  is  zero.  The  resulting  attractive  force  is  defined  as 

fai(q)  =  -VUal(q)  =  kae(q).  (12.11) 


Hence,  fal  converges  linearly  to  zero  when  the  robot  configuration  q  tends  to 
the  goal  configuration  qg . 

Alternatively,  it  is  possible  to  define  a  conical  attractive  potential  as 

Ua2(q)  =  ka\\e(q)\\.  (12.12) 


Also  Ua2  is  always  positive,  and  zero  in  qCJ.  The  corresponding  attractive  force 
is 


fa  2(9)  =  -VUa2(q)  =  fcg||^||, 


(12.13) 


that  is  constant  in  modulus.  This  represents  an  advantage  with  respect  to  the 
force  fal  generated  by  the  paraboloidal  attractive  potential,  which  tends  to 
grow  indefinitely  as  the  error  vector  increases  in  norm.  On  the  other  hand, 
fa2  is  indefinite  in  qg.  Figure  12.12  shows  the  shape  of  Uai  and  Ua2  in  the 
case  C  =  IR2,  with  k„  =  1. 

A  choice  that  combines  the  advantages  of  the  above  two  potentials  is  to 
define  the  attractive  potential  as  a  conical  surface  away  from  the  goal  and 
as  a  paraboloid  in  the  vicinity  of  qg.  In  particular,  by  placing  the  transition 
between  the  two  potentials  where  ||e(q)  ||  =  1  (i.e.,  on  the  surface  of  the  sphere 
of  unit  radius  centred  in  qg)  one  obtains  an  attractive  force  that  is  continuous 
for  any  q  (see  also  Problem  12.9). 


12.6.2  Repulsive  Potential 

The  repulsive  potential  Ur  is  added  to  the  attractive  potential  Ua  to  prevent 
the  robot  from  colliding  with  obstacles  as  it  moves  under  the  influence  of  the 
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Fig.  12.13.  The  equipotential  contours  of  the  repulsive  potential  Ur  in  the  range 
of  influence  of  a  polygonal  C-obstacle  in  C  =  IR2 ,  for  kr  =  1  and  7  =  2 


attractive  force  fa.  In  particular,  the  idea  is  to  build  a  barrier  potential  in 
the  vicinity  of  the  C-obstacle  region,  so  as  to  repel  the  point  that  represents 
the  robot  in  C. 

In  the  following,  it  will  be  assumed  that  the  C-obstacle  region  has  been 
partitioned  in  convex  components  COi,  i  =  1, . . .  ,p.  These  components  may 
coincide  with  the  C-obstacles  themselves;  this  happens,  for  example,  when  the 
robot  B  is  a  convex  polygon  (polyhedron)  translating  with  a  fixed  orientation 
in  IR2  (IR3)  among  convex  polygonal  (polyhedral)  obstacles  (see  Sect.  12.2.2). 
In  the  presence  of  non-convex  C-obstacles,  however,  it  is  necessary  to  per¬ 
form  the  decomposition  in  convex  components  before  building  the  repulsive 
potential. 

For  each  convex  component  COi ,  define  an  associated  repulsive  potential 


as 


Ur,i(q ) 


kr,i  f  1 
7  \Vi(q) 


1 

Vo,i 


7 

if  Vi(q)  <  vo,i 


0 


if  rji(q)  >  ?7oy, 


(12.14) 


where  kr< ,  >  0,  t)i(q)  =  min  q/eco4  ||q  —  q'\\  is  the  distance  of  q  from  COi >  Vo,i 
is  the  range  of  influence  of  COi  and  7  =  2,3, . . ..  The  potential  is  zero 
outside  and  positive  inside  the  range  of  influence  and  tends  to  infinity  as 
the  boundary  of  COi  is  approached,  more  abruptly  as  7  is  increased  (a  typical 
choice  is  7  =  2). 

When  C  =  IR2  and  the  convex  component  COi  is  polygonal,  an  equipo¬ 
tential  contour  of  L/r,i  (i.e. ,  the  locus  of  configurations  q  such  that  Urj  has 
a  certain  constant  value)  consists  of  rectilinear  tracts  that  are  parallel  to  the 
sides  of  the  polygon,  connected  by  arcs  of  circle  in  correspondence  of  the  ver¬ 
tices,  as  shown  in  Fig.  12.13.  Note  how  the  contours  get  closer  to  each  other 
in  the  proximity  of  the  C-obstacle  boundary,  due  to  the  hyperboloidic  profile 
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of  the  potential.  When  C  =  1R3  and  the  convex  component  COi  is  polyhedral, 
the  equipotential  surfaces  of  Urg  are  copies  of  the  faces  of  COi ,  connected  by 
patches  of  cylindrical  surfaces  in  correspondence  of  the  edges  and  spherical 
surfaces  in  correspondence  of  the  vertices  of  COi. 

The  repulsive  force  resulting  from  Urj  is 


fr,i(q)  =  -VUr,i(q)  =  { 


1  1 


v f(q)  \Vi(q)  vo,, 


7-1 


- Vry i{q)  if  rn(q)  <  rj0ti 


(12.15) 

if  r]i(q)  >  r)0>i. 


Denote  by  qm  the  configuration  of  COi  that  is  closer  to  q  (qm  is  uniquely 
determined  in  view  of  the  convexity  of  COi).  The  gradient  vector  Vr^(q), 
which  is  orthogonal  to  the  equipotential  contour  (or  surface)  passing  through 
q,  is  directed  as  the  half-line  originating  from  qm  and  passing  through  q.  If 
the  boundary  of  COi  is  piecewise  differentiable,  function  qj  is  differentiable 
everywhere  in  Cfree  and  fri  is  continuous  in  the  same  space.8 

The  aggregate  repulsive  potential  is  obtained  by  adding  up  the  individual 
potentials  associated  with  the  convex  components  of  CO: 


p 

Ur{q)  =  YJUr,i{q)-  (12.16) 

i=l 

If  r]i(qg)  >  ?7o,i  for  *  =  1, . . .  ,p  (i.e.,  if  the  goal  is  placed  outside  the  range  of 
influence  of  each  obstacle  component  COi),  the  value  of  the  aggregate  repulsive 
field  Ur  is  zero  in  qg.  In  the  following,  it  will  be  assumed  that  this  is  the  case. 


12.6.3  Total  Potential 

The  total  potential  Ut  is  obtained  by  superposition  of  the  attractive  and  the 
aggregate  repulsive  potentials: 

Ut(q)  =  Ua(q)  +  Ur(q).  (12.17) 

This  results  in  the  force  field 

p 

ft(q)  =  -VC4(g)  =  fa(q)  +  Y,  frM-  (12.18) 

i= 1 


8  Note  the  relevance  in  this  sense  of  the  assumption  that  the  component  COi  is 
convex.  If  it  were  otherwise,  there  would  exist  configurations  in  Cfree  for  which  qm 
would  not  be  uniquely  defined.  In  these  configurations,  belonging  by  definition  to 
the  generalized  Voronoi  diagram  V(Cfree),  function  r/i  would  not  be  differentiable, 
resulting  in  a  discontinuous  repulsive  force.  This  might  induce  undesired  effects 
on  the  planned  path  (for  example,  oscillations). 
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Fig.  12.14.  The  total  potential  in  C  =  1R2  obtained  by  superposition  of  a  hyper- 
boloidic  attractive  potential  and  a  repulsive  potential  for  a  rectangular  C-obstacle: 
left:  the  equipotential  contours,  right:  the  resulting  force  field 


Ut  clearly  has  a  global  minimum  in  qg ,  but  there  may  also  exist  some 
local  minima  where  the  force  field  is  zero.  Considering  for  simplicity  the  case 
C  =  IR2,  this  happens  in  the  ‘shadow  zone’  of  a  C-obstacle  when  the  repulsive 
potential  Urj  has  equipotential  contours  with  lower  curvature  (e.g.,  segments) 
than  the  attractive  potential  in  the  same  area.  See  for  example  Fig.  12.14, 
where  a  local  minimum  is  clearly  present  ‘below’  the  C-obstacle.  A  remarkable 
exception  is  the  case  ( sphere  world )  in  which  all  the  convex  components  COi  of 
the  C-obstacle  region  are  spheres.  In  this  situation,  the  total  potential  exhibits 
isolated  saddle  points  (where  the  force  field  is  still  zero)  but  no  local  minima. 

12.6.4  Planning  Techniques 

There  are  three  different  approaches  for  planning  collision-free  motions  on  the 
basis  of  a  total  artificial  potential  Ut  and  the  associated  force  field  ft  =  —  VC/t. 
They  are  briefly  discussed  below: 

1.  The  first  possibility  is  to  let 


r  =  ft(q),  (12.19) 

hence  considering  ft{q)  as  a  vector  of  generalized  forces  that  induce  a 
motion  of  the  robot  in  accordance  with  its  dynamic  model. 

2.  The  second  method  regards  the  robot  as  a  unit  point  mass  moving  under 
the  influence  of  ft(q),  as  in 


q  =  ft(q)- 


(12.20) 


3. 


The  third  possibility  is  to  interpret  the  force  field  ft{q)  as  a  desired  velocity 
for  the  robot,  by  letting 

(12.21) 


q  =  /*(<?)• 
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In  principle,  one  could  use  these  three  approaches  for  on-line  as  well  as 
off-line  motion  planning.  In  the  first  case,  (12.19)  directly  represents  con¬ 
trol  inputs  for  the  robot,  whereas  the  implementation  of  (12.20)  requires  the 
solution  of  the  inverse  dynamics  problem,  i.e.,  the  substitution  of  q  in  the 
robot  dynamic  model  to  compute  the  generalized  forces  t  that  realize  such 
accelerations.  Equation  (12.21)  can  instead  be  used  on-line  in  a  kinematic 
control  scheme,  in  particular  to  provide  the  reference  inputs  for  the  low-level 
controllers  that  are  in  charge  of  reproducing  such  generalized  velocities  as  ac¬ 
curately  as  possible.  In  any  case,  the  artificial  force  field  ft  represents,  either 
directly  or  indirectly,  a  true  feedback  control  that  guides  the  robot  towards 
the  goal,  while  trying  at  the  same  time  to  avoid  collisions  with  the  workspace 
obstacles  that  have  been  detected  by  the  sensory  system.  To  emphasize  this 
aspect,  on-line  motion  generation  based  on  artificial  potentials  is  also  referred 
to  as  reactive  planning. 

In  off-line  motion  planning,  configuration  space  paths  are  generated  by 
simulation,  i.e.,  integrating  numerically  the  robot  dynamic  model  if  (12.19)  is 
used,  or  directly  by  the  differential  equations  (12.20)  and  (12.21). 

In  general,  the  use  of  (12.19)  generates  smoother  paths,  because  with 
this  scheme  the  reactions  to  the  presence  of  obstacles  are  naturally  ‘fil¬ 
tered’  through  the  robot  dynamics.  On  the  other  hand,  the  strategy  repre¬ 
sented  by  (12.21)  is  faster  in  executing  the  motion  corrections  suggested  by 
the  force  field  /t,  and  may  thus  be  considered  safer.  The  characteristics  of 
scheme  (12.20)  are  clearly  intermediate  between  the  other  two.  Another  as¬ 
pect  to  be  considered  is  that  using  (12.21)  guarantees  (in  the  absence  of  local 
minima)  the  asymptotic  stability  of  qg  (i.e.,  the  robot  reaches  the  goal  with 
zero  velocity),  whereas  this  is  not  true  for  the  other  two  motion  generation 
strategies.  To  achieve  asymptotic  stability  with  (12.19)  and  (12.20),  a  damping 
term  proportional  to  the  robot  velocity  q  must  be  added  to  ft. 

In  view  of  the  above  discussion,  it  is  not  surprising  that  the  most  common 
choice  is  the  simple  numerical  integration  of  (12.21)  via  the  Euler  method: 

Qk+i  =  Qk  +  Tft(qk),  (12.22) 

where  qk  and  qk+l  represent  respectively  the  current  and  the  next  robot  con¬ 
figuration,  and  T  is  the  integration  step.  To  improve  the  quality  of  the  gen¬ 
erated  path,  it  is  also  possible  to  use  a  variable  T,  smaller  when  the  modulus 
of  the  force  field  ft  is  larger  (in  the  vicinity  of  obstacles)  or  smaller  (close  to 
the  destination  qg ).  Recalling  that  ft(q)  =  —VUt(q),  Eq.  (12.22)  may  be  eas¬ 
ily  interpreted  as  a  numerical  implementation  of  the  gradient  method  for  the 
minimization  of  Ut(q),  often  referred  to  as  the  algorithm  of  steepest  descent. 


12.6.5  The  Local  Minima  Problem 

Whatever  technique  is  used  to  plan  motions  on  the  basis  of  artificial  potentials, 
local  minima  of  Ut  —  where  the  total  force  field  ft  is  zero  —  represent  a 
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problem.  For  example,  if  (12.22)  is  used  and  the  generated  path  enters  the 
basin  of  attraction  of  a  local  minimum,  the  planning  process  is  bound  to  stop 
there,  without  reaching  the  goal  configuration.9  Actually,  the  same  problem 
may  occur  also  when  (12.19)  or  (12.20)  are  used  if  the  basin  of  attraction  of  the 
local  minimum  is  sufficiently  large.  On  the  other  hand,  as  noticed  previously, 
the  total  potential  Ut  obtained  by  the  superposition  of  an  attractive  and  a 
repulsive  potential  invariably  exhibits  local  minima,  except  for  very  particular 
cases.  This  means  that  motion  planning  methods  based  on  artificial  potentials 
are  not  complete  in  general,  because  it  may  happen  that  the  goal  configuration 
qg  is  not  reached  even  though  a  solution  exists. 


Best-first  algorithm 

A  simple  workaround  for  the  local  minima  problem  is  to  use  a  best-first  algo¬ 
rithm,  which  is  formulated  under  the  assumption  that  the  free  configuration 
space  Cfree  has  been  discretized  using  a  regular  grid.  In  general,  the  discretiza¬ 
tion  procedure  results  in  the  loss  of  some  boundary  regions  of  Cfree,  which  are 
not  represented  as  free  in  the  gridmap.  Each  free  cell  of  the  grid  is  assigned  the 
value  of  the  total  potential  Ut  computed  at  the  centroid  of  the  cell.  Planning 
proceeds  by  building  a  tree  T  rooted  at  the  start  configuration  qs.  At  each 
iteration,  the  leaf  with  the  minimum  value  of  Ut  is  selected  and  its  adjacent10 
cells  are  examined.  Those  that  are  not  already  in  T  are  added  as  children  of 
the  considered  leaf.  Planning  is  successful  when  the  cell  containing  the  goal  is 
reached  (in  this  case,  the  solution  path  is  built  by  tracing  back  the  arcs  of  the 
tree  from  qg  to  qs),  whereas  failure  is  reported  when  all  the  cells  accessible 
from  qs  have  been  explored  without  reaching  qg. 

The  above  best-first  algorithm  evolves  as  a  grid-discretizecl  version  of  the 
algorithm  of  steepest  descent  (12.22),  until  a  cell  is  reached  which  represents 
a  minimum  for  the  associated  total  potential.  If  it  is  a  local  minimum,  the 
algorithm  visits  (‘fills’)  its  entire  basin  of  attraction  and  finally  leaves  it,  reach¬ 
ing  a  point  from  which  planning  can  continue.  The  resulting  motion  planning 
method  is  resolution  complete,  because  a  solution  is  found  only  if  one  exists 
on  the  gridmap  that  represents  Cfree  by  defect.  In  general,  increasing  the  grid 
resolution  may  be  necessary  to  recover  the  possibility  of  determining  a  solu¬ 
tion.  In  any  case,  the  time  complexity  of  the  basin  filling  procedure,  which  is 
exponential  in  the  dimension  of  C  because  such  is  the  number  of  adjacent  cells 
to  a  given  one,  makes  the  best-first  algorithm  applicable  only  in  configuration 
spaces  of  low  dimension  (typically,  not  larger  than  3) . 


9  The  probability  of  reaching  a  saddle  point  instead  is  extremely  low;  besides,  any 
perturbation  would  allow  the  planner  to  exit  such  a  point. 

10  Various  definitions  of  adjacency  may  be  adopted.  For  example,  in  ]R2  one  may 
use  1-adjacency  or  2-adjacency.  In  the  first  case,  each  cell  c  has  four  adjacent 
cells,  while  in  the  second  they  are  eight.  The  resulting  paths  will  obviously  reflect 
this  fact. 
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A  more  effective  approach  is  to  include  in  the  best-first  method  a  ran¬ 
domized  mechanism  for  evading  local  minima.  In  practice,  one  implements  a 
version  of  the  best-first  algorithm  in  which  the  number  of  iterations  aimed  at 
filling  the  basin  of  attraction  of  local  minima  is  bounded.  When  the  bound 
is  reached,  a  sequence  of  random  steps  ( random  walk)  is  taken.  This  usually 
allows  the  planner  to  evade  the  basin  in  a  much  shorter  time  than  would 
be  needed  by  the  complete  filling  procedure.  As  the  probability  of  evading 
the  basin  of  attraction  of  a  local  minimum  approaches  1  when  the  number 
of  random  steps  tends  to  infinity,  this  randomized  best-first  method  is  proba¬ 
bilistically  complete  (in  addition  to  being  resolution  complete). 


Navigation  functions 

Although  the  best-first  algorithm  (in  its  basic  or  randomized  version)  repre¬ 
sents  a  solution  to  the  problem  of  local  minima,  it  may  generate  paths  that 
are  very  inefficient.  In  fact,  with  this  strategy  the  robot  will  still  enter  (and 
then  evade)  the  basin  of  attraction  of  any  local  minimum  located  on  its  way 
to  goal.  A  more  radical  approach  is  based  on  the  use  of  navigation  functions , 
i.e. ,  artificial  potentials  that  have  no  local  minima.  As  already  mentioned,  if 
the  C-obstacles  are  spheres  this  property  already  holds  for  the  potential  Ut 
defined  by  superposition  of  an  attractive  and  a  repulsive  field.  A  first  possi¬ 
bility  would  then  be  to  approximate  by  excess  all  C-obstacles  with  spheres, 
and  use  the  total  potential  Ut ■  Clearly,  such  an  approximation  may  severely 
reduce  the  free  configuration  space  Cfree,  and  even  destroy  its  connectedness; 
for  example,  imagine  what  would  happen  in  the  case  depicted  in  Fig.  12.4. 

In  principle,  a  mathematically  elegant  way  to  define  a  navigation  function 
consists  of  building  first  a  differentiable  homeomorphism  (a  diffeomorphism ) 
that  maps  the  C-obstacle  region  to  a  collection  of  spheres,  then  generating  a 
classical  total  potential  in  the  transformed  space,  and  finally  mapping  it  back 
to  the  original  configuration  space  so  as  to  obtain  a  potential  free  of  local 
minima.  If  the  C-obstacles  are  star-shaped ,n  such  a  diffeomorphism  actually 
exists,  and  the  procedure  outlined  above  provides  in  fact  a  navigation  function. 
Another  approach  is  to  build  the  potential  using  harmonic  functions,  that  are 
the  solutions  of  a  particular  differential  equation  that  describes  the  physical 
process  of  heat  transmission  or  fluid  dynamics. 

Generating  a  navigation  function  is,  however,  computationally  cumber¬ 
some,  and  the  associated  planning  methods  are  thus  mainly  of  theoretical  in¬ 
terest.  A  notable  exception,  at  least  in  low-dimensional  configuration  spaces, 
is  the  numerical  navigation  function.  This  is  a  potential  built  on  a  gridmap 
representation  of  Cfree  by  assigning  value  0  to  the  cell  containing  qg ,  value  1 
to  its  adjacent  cells,  value  2  to  the  unvisited  cells  among  those  adjacent  to 


11  A  subset  S  C  IR”  is  said  to  be  star-shaped  if  it  is  homeomorphic  to  the  closed 
unit  sphere  in  JR"  and  has  a  point  p  ( centre )  such  that  any  other  point  in  S  may 
be  joined  to  p  by  a  segment  that  is  entirely  contained  in  S. 


554  12  Motion  Planning 


2 

1 

2 

3 

4 

5 

6 

7 

8 

9 

1 

0 

1 

6 

7 

8 

9 

10 

2 

1 

2 

□ 

7 

8 

■ 

10 

11 

3 

3 

4 

5 

6 

7 

8 

■ 

12 

4 

5 

6 

7 

12 

13 

5 

6 

7 

6 

7 

8 

9 

10 

11 

12 

6 

7 

8 

7 

8 

9 

10 

11 

12 

13 

14  15 


Fig.  12.15.  An  example  of  numerical  navigation  function  in  a  simple  two- 
dimensional  gridmap  using  1-adjacency.  Cells  in  gray  denote  a  particular  solution 
path  obtained  by  following  from  the  start  (cell  12)  the  steepest  descent  of  the  po¬ 
tential  on  the  gridmap 


cells  with  potential  1,  and  so  on.  To  understand  better  the  procedure,  one 
may  visualize  a  wavefront  that  originates  at  qg  and  expands  according  to  the 
adopted  adjacency  definition  ( wavefront  expansion  algorithm).  It  is  easy  to 
realize  that  the  obtained  potential  is  free  of  local  minima,  and  therefore  its 
use  in  conjunction  with  the  algorithm  of  steepest  descent  provides  a  motion 
planning  method  that  is  complete  in  resolution  (see  Fig.  12.15). 

Finally,  it  should  be  mentioned  that  the  use  of  navigation  functions  is 
limited  to  off-line  motion  planning,  because  their  construction  -  be  they 
continuous  or  discrete  in  nature  —  requires  the  a  priori  knowledge  of  the  ge¬ 
ometry  and  pose  of  the  workspace  obstacles.  As  for  on-line  motion  planning, 
in  which  the  obstacles  are  gradually  reconstructed  via  sensor  measurements 
as  the  robot  moves,  the  incremental  construction  of  a  total  potential  by  super¬ 
position  of  attractive  and  repulsive  fields  represents  a  simple,  often  effective 
method  to  generate  collision-free  motions,  even  though  completeness  cannot 
be  claimed.  In  any  case,  motion  planning  based  on  artificial  potentials  be¬ 
long  to  the  single-query  category,  because  the  attractive  component  of  the 
potential  depends  on  the  goal  configuration  qg. 

12.7  The  Robot  Manipulator  Case 

Generating  collision-free  movements  for  robot  manipulators  is  a  particularly 
important  category  of  motion  planning  problems.  In  general,  the  computa¬ 
tional  complexity  associated  with  this  problem  is  substantial,  due  to  the  high 
dimension  of  the  configuration  space  (typically  n  >  4)  and  to  the  presence  of 
rotational  DOFs  (revolute  joints). 

It  is  sometimes  possible  to  reduce  the  dimension  of  the  configuration  space 
C  approximating  by  excess  the  size  of  the  robot.  For  example,  in  a  six-DOF 
anthropomorphic  manipulator  one  can  replace  the  last  three  links  of  the  kine¬ 
matic  chain  (the  spherical  wrist)  and  the  end-effector  with  the  volume  they 
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‘sweep’  when  the  corresponding  joints  move  across  their  whole  available  range. 
The  dimension  of  the  configuration  space  becomes  three,  as  planning  concerns 
only  the  base,  shoulder  and  elbow  joints,  while  the  wrist  can  move  arbitrarily. 
Clearly,  this  approximation  is  conservative,  and  hence  acceptable  only  if  the 
aforementioned  volume  is  small  with  respect  to  the  workspace  of  the  manip¬ 
ulator. 

In  the  presence  of  rotational  DOFs,  the  other  complication  is  the  shape 
of  the  C-obstacles,  which  is  complex  even  for  simple  workspace  obstacles  due 
to  the  strong  nonlinearity  introduced  by  the  manipulator  inverse  kinematics 
(recall  Fig.  12.4).  Apart  from  the  intrinsic  difficulty  of  computing  C-obstacles, 
their  non-polyhedral  shape  does  not  allow  the  application  of  the  planning 
methods  presented  in  Sects.  12.3  and  12.4. 

The  most  convenient  choice  for  off-line  planning  is  represented  by  prob¬ 
abilistic  methods,  which  exhibit  the  best  performance  in  high-dimensional 
configuration  spaces.  Moreover,  as  the  computation  of  the  C-obstacles  is  not 
required,  these  planners  are  not  affected  by  their  shape.  However,  it  should  be 
noted  that  collision  checking  —  which  is  an  essential  tool  for  probabilistic  mo¬ 
tion  planning  becomes  more  onerous  as  the  number  of  DOFs  is  increased. 

For  on-line  planning,  the  best  results  are  obtained  by  a  suitable  adapta¬ 
tion  of  the  method  based  on  artificial  potentials.  In  particular,  to  avoid  the 
computation  of  C-obstacles  and  at  the  same  time  plan  in  a  space  of  reduced 
dimension,  the  potential  is  directly  built  in  the  workspace  VV  =  IR^  rather 
than  in  the  configuration  space  C,  and  acts  on  a  set  of  control  points  located 
on  the  manipulator.  Among  these  is  included  a  point  that  represents  the  end- 
effector  (to  which  is  assigned  the  goal  of  the  motion  planning  problem)  and  at 
least  one  point  (possibly  variable  in  time)  for  each  body  of  the  linkage.  While 
the  attractive  potential  only  influences  the  end-effector  representative  point, 
the  repulsive  potential  acts  on  all  control  points.  As  a  consequence,  the  arti¬ 
ficial  potentials  used  in  this  scheme  are  actually  two:  an  attractive-repulsive 
field  for  the  end-effector,  and  a  repulsive  field  for  the  other  control  points 
distributed  on  the  manipulator  links. 

As  before,  different  approaches  may  be  used  to  convert  the  force  fields 
generated  by  the  artificial  potentials  to  commands  for  the  manipulator.  De¬ 
note  by  Pi{q),  i  =  1  the  coordinates  in  W  of  the  P  control  points 

in  correspondence  of  the  configuration  q  of  the  manipulator.  In  particular, 
p1,...,pP_1  are  the  control  points  located  on  the  manipulator  links,  sub¬ 
ject  only  to  the  repulsive  potential  Ur ,  while  pP  is  the  control  point  for  the 
end-effector,  which  is  subject  to  the  total  potential  Ut  =  Ua  +  Ur. 

A  first  possibility  is  to  impose  to  the  robot  joints  the  generalized  forces 
which  would  result  from  the  combined  action  of  the  various  force  fields  acting 
on  the  control  points  in  the  workspace,  according  to 


p- 1 

T  =  -  E  Ji(^Ur (Pi)  -  jl(q)VUt(pP), 

i= 1 


(12.23) 
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Fig.  12.16.  Examples  of  motion  planning  via  artificial  potentials  acting  on  con¬ 
trol  points  for  a  planar  3R  manipulator;  left:  planning  is  successful  and  leads  to  a 
collision-free  motion  between  the  start  S  and  the  goal  G,  right:  a  failure  is  reported 
because  the  manipulator  is  stuck  at  a  force  equilibrium 


where  Ji(q ),  i  =  1  denotes  the  Jacobian  of  the  direct  kinematics 

function  associated  with  the  control  point  Pi{q)- 

Alternatively,  a  purely  kinematic  planning  scheme  is  obtained  by  letting 

p- 1 

q  =  ~Y,J? -  Jl(q)VUt(pP)  (12.24) 

i= 1 


and  feeding  these  joint  velocities  to  the  low-level  control  loops  as  reference 
signals.  Note  that  Eq.  (12.24)  represents  a  gradient-based  minimization  step 
in  the  configuration  space  C  of  a  combined  potential  defined  in  the  workspace 
W.  In  fact,  a  potential  function  acting  on  a  control  point  in  the  workspace  may 
be  seen  as  a  composite  function  of  q  through  the  associated  direct  kinematics 
relationship,  and  the  Jacobian  transpose  jf  ( q ),  i  =  1, . . , ,  P,  maps  a  gradient 
in  W  to  a  gradient  in  C.  In  formulae: 


VqJ7(Pi) 


^dU(pi(q))^T 


(  dU{Pi )  dp,  \  T 

\  dPi  dq  ) 


jf  (?)  Vt/(p?;), 


for  i  =  1, . . . ,  P. 

The  above  two  schemes  can  be  considered  respectively  the  transposition 
of  (12.19)  and  (12.21),  and  therefore  they  inherit  the  same  characteristics. 
In  particular,  when  (12.23)  is  used  the  motion  corrections  prescribed  by  the 
force  fields  are  filtered  through  the  dynamic  model  of  the  manipulator,  and 
smoother  movements  can  be  expected.  The  kinematic  scheme  (12.24)  instead 
is  faster  in  realizing  such  corrections. 

Finally,  it  should  be  mentioned  that  the  use  of  artificial  potentials  that  are 
defined  in  the  workspace  may  aggravate  the  local  minima  problem.  In  fact, 
the  various  forces  (either  purely  attractive  or  repulsive-attractive)  acting  on 
the  control  points  may  neutralize  each  other  at  the  joint  level,  blocking  the 
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manipulator  in  a  configuration  ( force  equilibrium )  where  no  control  point  is  at 
a  local  minimum  of  the  associated  potential  (see  Fig.  12.16).  As  consequence, 
it  is  always  advisable  to  use  workspace  potential  fields  in  conjunction  with  a 
randomized  best-first  algorithm. 
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In  the  last  three  decades,  the  literature  on  motion  planning  has  grown  con¬ 
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cell  decomposition  is  described  in  [122],  while  [197]  and  [33]  are  among  the  gen¬ 
eral  planning  methods  via  decomposition  mentioned  at  the  end  of  Sect.  12.4.1. 
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The  use  of  artificial  potentials  for  on-line  motion  planning  was  pioneered 
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its  numerical  version  on  a  gridmap  was  described  in  [17],  together  with  the 
best-first  algorithm,  also  in  its  randomized  version. 

For  other  aspects  of  the  motion  planning  problem  that  are  merely  hinted 
at  (nonholonomic  motion  planning)  or  simply  ignored  (managing  uncertainty, 
mobile  obstacles)  in  this  chapter,  the  reader  can  consult  many  excellent  books, 
going  from  the  classical  treatment  in  [122],  through  [123],  to  the  most  recent 
texts  [45,  145,  124], 


Problems 

12.1.  Describe  the  nature  (including  the  dimension)  of  the  configuration  space 
for  a  mobile  manipulator  consisting  of  a  unicycle-like  vehicle  carrying  a  six- 
DOF  anthropomorphic  arm,  providing  a  choice  of  generalized  coordinates  for 
the  system. 

12.2.  With  reference  to  a  2R  manipulator,  modify  the  definition  (12.2)  of 
configuration  space  distance  so  as  to  take  into  account  the  fact  that  the  ma¬ 
nipulator  posture  does  not  change  if  the  joint  variables  q\  and  <72  are  increased 
(or  decreased)  by  a  multiple  of  27t. 

12.3.  Consider  a  polygonal  robot  translating  at  a  fixed  orientation  in  IR2 
among  polygonal  obstacles.  Build  an  example  showing  that  the  same  C- 
obstacle  region  may  correspond  to  robot  and  obstacles  of  different  shapes. 
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12.4.  With  reference  to  the  second  workspace  shown  in  Fig.  12.4,  give  the 
numerical  value  of  three  configurations  of  the  manipulator  that  lie  in  the  three 
connected  components  of  Cfree-  Moreover,  sketch  the  manipulator  posture  for 
each  of  these  configurations. 

12.5.  Discuss  the  basic  steps  of  an  algorithm  for  computing  the  generalized 
Voronoi  diagram  of  a  limited  polygonal  subset  of  IR2.  [Hint:  a  simple  algorithm 
is  obtained  by  considering  all  the  possible  side-side,  side-vertex  and  vertex- 
vertex  pairs,  and  generating  the  elementary  arcs  of  the  diagram  on  the  basis 
of  the  intersections  of  the  associated  equidistance  contours.] 

12.6.  For  the  motion  planning  method  via  exact  cell  decomposition,  give  an 
example  in  which  the  path  obtained  as  a  broken  line  joining  the  midpoints 
of  the  common  boundary  of  the  channel  cells  goes  through  a  vertex  of  the 
C-obstacle  region,  and  propose  an  alternative  procedure  for  extracting  a  free 
path  from  the  channel.  [Hint:  build  a  situation  in  which  the  channel  from  cs 
to  cg  contains  a  cell  for  which  the  entrance  and  the  exit  boundary  lie  on  the 
same  side.] 

12.7.  Implement  in  a  computer  program  the  PRM  method  for  a  2R  planar 
robot  moving  among  circular  workspace  obstacles.  The  program  receives  as 
input  a  geometrical  description  of  the  obstacles  (centre  and  radius  of  each 
obstacle)  as  well  as  the  start  and  goal  configurations,  and  terminates  after 
a  maximum  number  of  iterations.  If  a  solution  path  has  been  found,  it  is 
given  as  output  together  with  the  associated  PRM;  otherwise,  a  failure  is 
reported.  Discuss  the  performance  of  the  method  with  respect  to  the  choice 
of  configuration  space  distance  (for  example,  (12.1)  or  (12.2)). 

12.8.  Implement  in  a  computer  program  the  RRT  method  for  a  circular¬ 
shaped  unicycle  moving  among  square  obstacles,  with  the  motion  primitives 
defined  as  in  (12.9).  The  program  receives  as  input  the  start  and  goal  config¬ 
urations,  in  addition  to  a  geometrical  description  of  the  obstacles  (centre  and 
side  of  each  obstacle),  and  terminates  after  a  maximum  number  of  iterations. 
If  a  solution  path  has  been  found,  it  is  given  as  output  together  with  the 
associated  RRT;  otherwise,  a  failure  is  reported.  Build  a  situation  in  which 
the  method  cannot  find  a  solution  because  of  the  limitations  inherent  to  the 
chosen  motion  primitives. 

12.9.  For  the  case  C  =  IR2,  build  a  continuously  differentiable  attractive  po¬ 
tential  having  a  paraboloidal  profile  inside  the  circle  of  radius  p  and  a  conical 
profile  outside.  [Hint:  modify  the  expression  (12.12)  of  the  conical  potential 
using  a  different  constant  fcf,  in  place  of  ka,  which  already  characterizes  the 
paraboloidal  potential.] 

12.10.  For  the  case  C  =  IR2,  prove  that  the  total  potential  Ut  may  exhibit 
a  local  minimum  in  areas  where  the  equipotential  contours  of  the  repulsive 
potential  Ur  have  lower  curvature  than  those  of  the  attractive  potential  Ua. 
[Hint:  consider  a  polygonal  C-obstacle  and  use  a  geometric  construction.] 
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12.11.  Consider  a  point  robot  moving  in  a  planar  workspace  containing  three 
circular  obstacles,  respectively  of  radius  1,  2  and  1  and  centre  in  (2, 1),  (—1, 3) 
and  (1,  —2).  The  goal  is  the  origin  of  the  workspace  reference  frame.  Build  the 
total  potential  Ut  resulting  from  the  superposition  of  the  attractive  potential 
Ua  to  the  goal  and  the  repulsive  potential  Ur  from  the  obstacles,  and  derive 
the  corresponding  artificial  force  field.  Moreover,  compute  the  coordinates  of 
the  saddle  points  of  Ut. 

12.12.  Discuss  the  main  issues  arising  from  the  application  of  the  artificial 
potential  technique  for  planning  on-line  the  motion  of  an  omnidirectional  cir¬ 
cular  robot.  Assume  that  the  robot  is  equipped  with  a  rotating  laser  range 
finder  placed  at  its  centre  that  measures  the  distance  between  the  sensor  and 
the  closest  obstacles  along  each  direction.  If  the  distance  is  larger  than  the 
maximum  measurable  range  R,  the  sensor  returns  R  as  a  reading.  Sketch  a 
possible  extension  of  the  method  to  a  unicycle-like  mobile  robot. 

12.13.  Implement  in  a  computer  program  the  motion  planning  method  based 
on  the  numerical  navigation  function.  The  program  receives  as  input  a  two- 
dimensional  gridmap,  in  which  some  of  the  cells  are  labelled  as  ‘obstacles’,  with 
the  specification  of  the  start  and  the  goal  cells.  If  the  algorithm  is  successful, 
a  sequence  of  free  cells  from  the  start  to  the  goal  is  provided  as  output.  With 
the  aid  of  some  examples,  compare  the  average  length  of  the  solution  paths 
obtained  using  1-adjacency  and  2-adjacency  to  build  the  navigation  function. 
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Linear  Algebra 


Since  modelling  and  control  of  robot  manipulators  requires  an  extensive  use 
of  matrices  and  vectors  as  well  as  of  matrix  and  vector  operations,  the  goal 
of  this  appendix  is  to  provide  a  brush-up  of  linear  algebra. 


A.l  Definitions 

A  matrix  of  dimensions  {in  x  n),  with  m  and  n  positive  integers,  is  an  array 


of  elements  arranged  into  m  rows 

and  n 

columns : 

"  an 

a\2 

O' In 

A=ia^i=l,...,m  = 

<221 

022 

02  n 

(A.l) 

j  =  l,...,n 

-  am  i 

Om  2  •  ■  • 

Omn  - 

If  m  =  n,  the  matrix  is  said  to  be  square ; 

if  in  <  n, 

the  matrix  has 

more 

columns  than  rows;  if  m  >  n  the  matrix  has  more  rows  than  columns.  Further, 
if  n  =  1,  the  notation  (A.l)  is  used  to  represent  a  (column)  vector  a  of 
dimensions  (to  x  l);1  the  elements  are  said  to  be  vector  components. 

A  square  matrix  A  of  dimensions  (n  x  n)  is  said  to  be  upper  triangular  if 
an  =  0  for  i  >  j: 

0\  n 

&2  n 
•  ? 

O nn  - 

the  matrix  is  said  to  be  lower  triangular  if  a7J  =  0  for  i  <  j. 


A  = 


a  ii  a  12 
0  022 


0  0 


1  According  to  standard  mathematical  notation,  small  boldface  is  used  to  denote 
vectors  while  capital  boldface  is  used  to  denote  matrices.  Scalars  are  denoted  by 
roman  characters. 
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An  (n  x  n)  square  matrix  A  is  said  to  be  diagonal  if  fty  =  0  for  i  ^  j,  i.e., 


an 

0 


0 

«22 


L  o  o 


0  - 
o 


=  diag{an,a22,  •  •  •  ,ann}. 


If  an  (n  x  n)  diagonal  matrix  has  all  unit  elements  on  the  diagonal  {an  =  1), 
the  matrix  is  said  to  be  identity  and  is  denoted  by  In.  A  matrix  is  said  to  be 
null  if  all  its  elements  are  null  and  is  denoted  by  O.  The  null  column  vector 
is  denoted  by  0. 

The  transpose  AT  of  a  matrix  A  of  dimensions  (to  x  n)  is  the  matrix  of 
dimensions  {nxm)  which  is  obtained  from  the  original  matrix  by  interchanging 
its  rows  and  columns: 


At 


a  n  a2i 

Oi2  022 


Qml 

»m2 


(A.2) 


L  Clin  Cl2n 


a 


mn  J 


The  transpose  of  a  column  vector  a  is  the  row  vector  aT . 

An  (n  x  n)  square  matrix  A  is  said  to  be  symmetric  if  AT  =  A ,  and  thus 
a-ij  —  aji- 


On  Oi2 
Ol2  022 


Oin 
CL2  n 


LOi„  02  n 


a 


nn  J 


An  {n  x  n)  square  matrix  A  is  said  to  be  skew-symmetric  if  AT  =  — A ,  and 
thus  aij  =  — Oji  for  i  ^  j  and  an  =  0,  leading  to 


A  = 


proper  dimensions: 


A  = 


-  0 

Ol2 

O'ln 

— ai2 

0 

■  ■  <22  n 

-  CL\n 

—  <22n 

..  0  . 

a  matrix  whose  elements 

■An 

A-12 

Ai„  ' 

A-21 

A-22 

A2n 

-  Ami 

Am2 

4 

•  -^mn  -i 

2 


Subscript  n  is  usually  omitted  if  the  dimensions  are  clear  from  the  context. 
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A  partitioned  matrix  may  be  block-triangular  or  block-diagonal.  Special  par¬ 
titions  of  a  matrix  are  that  by  columns 

A=[ai  a2  ...  an] 

and  that  by  rows 


A  = 


Given  a  square  matrix  A  of  dimensions  (n  x  n),  the  algebraic  complement 
A^  of  element  is  the  matrix  of  dimensions  ((n  —  1)  x  (n  —  1))  which  is 
obtained  by  eliminating  row  i  and  column  j  of  matrix  A. 

A. 2  Matrix  Operations 

The  trace  of  an  (n  x  n )  square  matrix  A  is  the  sum  of  the  elements  on  the 
diagonal: 

n 

Tr  (A)  =  ^2au.  (A.3) 

i=l 

Two  matrices  A  and  B  of  the  same  dimensions  (m  x  n)  are  equal  if  a,l3  = 
bij.  If  A  and  B  are  two  matrices  of  the  same  dimensions,  their  sum  is  the 
matrix 

C  =  A  +  B  (A. 4) 

whose  elements  are  given  by  Cij  =  aij  +  bij.  The  following  properties  hold: 

A  +  O  =  A 
A  +  B  =  B  +  A 

(A  +  B)  +  C  =  A  +  (B  +  C). 

Notice  that  two  matrices  of  the  same  dimensions  and  partitioned  in  the  same 
way  can  be  summed  formally  by  operating  on  the  blocks  in  the  same  position 
and  treating  them  like  elements. 

The  product  of  a  scalar  a  by  an  (mxn)  matrix  A  is  the  matrix  a  A  whose 
elements  are  given  by  aaij.  If  A  is  an  (n  x  n )  diagonal  matrix  with  all  equal 
elements  on  the  diagonal  ( an  =  a),  it  follows  that  A  =  aln. 

If  A  is  a  square  matrix,  one  may  write 

A  =  As  +  Aa  (A. 5) 

As=l-{A  +  AT) 


where 


(A.6) 
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is  a  symmetric  matrix  representing  the  symmetric  part  of  A ,  and 

Aa  =  ^(A-AT)  (A. 7) 

is  a  skew-symmetric  matrix  representing  the  skew-symmetric  part  of  A. 

The  row-by-column  product  of  a  matrix  A  of  dimensions  (to  xp)  by  a 
matrix  B  of  dimensions  ( p  x  n)  is  the  matrix  of  dimensions  (to  x  n) 


C  =  AB  (A. 8) 

whose  elements  are  given  by  Cij  =  Y^k=i  aikbkj-  The  following  properties  hold: 

A  —  Alp  —  I  mA 
A(BC)  =  ( AB)C 
A(B  +  C)  =  AB  +  AC 
(A  +  B)C  =  AC  +  BC 
(. AB)t  =  BtAt. 

Notice  that,  in  general,  AB  BA ,  and  AB  =  O  does  not  imply  that  A  =  O 
or  B  =  O;  further,  notice  that  AC  =  BC  does  not  imply  that  A  =  B. 

If  an  (to  x  p)  matrix  A  and  a  (p  x  n)  matrix  B  are  partitioned  in  such  a 
way  that  the  number  of  blocks  for  each  row  of  A  is  equal  to  the  number  of 
blocks  for  each  column  of  B ,  and  the  blocks  A,^  and  B^j  have  dimensions 
compatible  with  product,  the  matrix  product  AB  can  be  formally  obtained  by 
operating  by  rows  and  columns  on  the  blocks  of  proper  position  and  treating 
them  like  elements. 

For  an  (n  x  n)  square  matrix  A,  the  determinant  of  A  is  the  scalar  given 
by  the  following  expression,  which  holds  Vz  =  1 , ,n: 

n 

det(A)  =  y^ajj(-l)t+Jdet(Afa)).  (A. 9) 

3  =  1 

The  determinant  can  be  computed  according  to  any  row  i  as  in  (A. 9);  the 
same  result  is  obtained  by  computing  it  according  to  any  column  j.  If  n  =  1, 
then  det(an)  =  an.  The  following  property  holds: 

det(A)  =  det(Ar). 

Moreover,  interchanging  two  generic  columns  p  and  q  of  a  matrix  A  yields 

det([oi ...  op  ...  ag  ...  on])  =  -det([ai . . .  aq  . . .  ap  . . .  an)) . 

As  a  consequence,  if  a  matrix  has  two  equal  columns  (rows),  then  its  deter¬ 
minant  is  null.  Also,  it  is  det(aA)  =  a”det(A). 

Given  an  (to  x  n)  matrix  A,  the  determinant  of  the  square  block  obtained 
by  selecting  an  equal  number  k  of  rows  and  columns  is  said  to  be  fc-order  minor 
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of  matrix  A.  The  minors  obtained  by  taking  the  first  k  rows  and  columns  of 
A  are  said  to  be  principal  minors. 

If  A  and  B  are  square  matrices,  then 

clet  (AB)  =  det(A)det(B).  (A.  10) 

If  A  is  an  (n  x  n )  triangular  matrix  (in  particular  diagonal),  then 

n 

det(A)  =  JJaij. 

2  =  1 

More  generally,  if  A  is  block-triangular  with  m  blocks  Arl  on  the  diagonal, 
then 

m 

det(A)  =  JJdet(Aji). 

2  =  1 

A  square  matrix  A  is  said  to  be  singular  when  det(A)  =  0. 

The  rank  g{A)  of  a  matrix  A  of  dimensions  (m  x  n)  is  the  maximum 
integer  r  so  that  at  least  a  non-null  minor  of  order  r  exists.  The  following 
properties  hold: 

g{A)  <  m in  (to  ,  n} 

e(A)  =  g(AT) 

g{ATA)  =  g{A) 

g{AB)  <  min{g(A),  g(B)}. 

A  matrix  so  that  g(A)  =  min {m,n}  is  said  to  be  full-rank. 

The  adjoint  of  a  square  matrix  A  is  the  matrix 

Adj  A  =  [(-l)^det(AW))]^=  1  ;n.  (A. 11) 

j  =  !,•••,  n 

An  (n  x  n)  square  matrix  A  is  said  to  be  invertible  if  a  matrix  AT1  exists, 
termed  inverse  of  A,  so  that 

A'1  A  =  AA-1  =  In. 

Since  g(In)  =  n,  an  ( n  x  n)  square  matrix  A  is  invertible  if  and  only  if 
g(A)  =  n,  i.e.,  clet(A)  0  (nonsingular  matrix).  The  inverse  of  A  can  be 
computed  as 

A  l  =  det^A) Ad'  A'  <A'12> 

The  following  properties  hold: 

(A”1)"1  =  A 

(a2")”1  =  (A-y. 
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If  the  inverse  of  a  square  matrix  is  equal  to  its  transpose 

At  =  A'1  (A. 13) 

then  the  matrix  is  said  to  be  orthogonal ;  in  this  case  it  is 

AAT  =  ATA  =  I.  (A.  14) 

A  square  matrix  A  is  said  idempotent  if 


AA  =  A.  (A. 15) 

If  A  and  B  are  invertible  square  matrices  of  the  same  dimensions,  then 
(AB)-1  =  B~1A~1.  (A. 16) 


Given  n  square  matrices  AVI  all  invertible,  the  following  expression  holds: 

(diag{Au, . . . ,  Ann })  1  =  diag {A^1, . . . ,  A~x}. 

where  diagjAn, . . . ,  Ann}  denotes  the  block-diagonal  matrix. 

If  A  and  C  are  invertible  square  matrices  of  proper  dimensions,  the  fol¬ 
lowing  expression  holds: 

(A  +  BCD)-1  =  A-1  -  A~1B{DA~1B  +  C_1)_1.DA_1, 

where  the  matrix  DA-XB  +  C”1  must  be  invertible. 

If  a  block-partitioned  matrix  is  invertible,  then  its  inverse  is  given  by  the 
general  expression 

A  D 
C  B 

where  A  =  B  —  C A-1  D ,  E  =  A  XD  and  F  =  CA^1,  under  the  assumption 
that  the  inverses  of  matrices  A  and  A  exist.  In  the  case  of  a  block-triangular 
matrix,  invertibility  of  the  matrix  requires  invertibility  of  the  blocks  on  the 
diagonal.  The  following  expressions  hold: 


A 

O 

-1 

A"1  O 

C 

B 

~B~1CA1  B1 

A 

D 

-1 

'A-1  —A~1DB~1 

O 

B 

O  B~x 

The  derivative  of  an  (to  x  n )  matrix  A(t),  whose  elements  aij(t)  are  dif¬ 
ferentiable  functions,  is  the  matrix 


A~l  +  EA~XF 

EA~X 

- A~XF 

A-1 

(A.  17) 


. ,  m 
• » n 


(A- 18) 
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If  an  (n  x  n)  square  matrix  A(t)  is  so  that  g(A(t))  =  n  Mt  and  its  elements 
are  differentiable  functions,  then  the  derivative  of  the  inverse  of  A{t) 
is  given  by 

jtA-\t)  =  -A-\t)A{t)A-\t).  (A.  19) 

Given  a  scalar  function  f(x),  endowed  with  partial  derivatives  with  respect 
to  the  elements  Xi  of  the  (n  x  1)  vector  x ,  the  gradient  of  function  /  with 
respect  to  vector  x  is  the  (n  x  1)  column  vector 


VJ  (*) 


(  9f{x) 

\T- 

'  df(x) 

df(x) 

df(x) ' 

\ 

dxi 

dx  2 

dxn 

(A. 20) 


Further,  if  x(t)  is  a  differentiable  function  with  respect  to  t,  then 

f(x)  =  Jtf(xtt))  =  =  Vxf{x)x.  (A. 21) 


Given  a  vector  function  g{x)  of  dimensions  (to  x  1),  whose  elements  gi  are 
differentiable  with  respect  to  the  vector  x  of  dimensions  (n  x  1),  the  Jacobian 
matrix  (or  simply  Jacobian )  of  the  function  is  defined  as  the  (to  x  n)  matrix 


Jg(x) 


dg(x) 

dx 


-  dgijx )  - 
dx 

dg2(x) 

dx 


dgm{x) 

L  dx  J 


If  x(t)  is  a  differentiable  function  with  respect  to  t,  then 


(A. 22) 


g(x) 


|9(x(())  =  gi  =  Js(x)i 


(A. 23) 


A. 3  Vector  Operations 

Given  n  vectors  Xi  of  dimensions  (to  x  1),  they  are  said  to  be  linearly  inde¬ 
pendent  if  the  expression 


kiXi  +  k2x 2  +  . . .  +  knxn  =  0 


holds  true  only  when  all  the  constants  ki  vanish.  A  necessary  and  sufficient 
condition  for  the  vectors  xi,  x2  ■  ■  ■ ,  xn  to  be  linearly  independent  is  that  the 
matrix 


A=[x  i  x2  ...  xn] 
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has  rank  n;  this  implies  that  a  necessary  condition  for  linear  independence 
is  that  n  <  m.  If  instead  g{A)  =  r  <  n,  then  only  r  vectors  are  linearly 
independent  and  the  remaining  n  —  r  vectors  can  be  expressed  as  a  linear 
combination  of  the  previous  ones. 

A  system  of  vectors  A”  is  a  vector  space  on  the  field  of  real  numbers  IR  if 
the  operations  of  sum  of  two  vectors  of  X  and  product  of  a  scalar  by  a  vector 
of  X  have  values  in  X  and  the  following  properties  hold: 

x  +  y  =  y  +  x  Vx,y  £  X 

(x  +  y)  +  z  =  x  +  (y  +  z)  \/x,y,z£X 

30  £  X  :£c  +  0  =  £c  Mx  £  X 

\/x  £  X ,  3(— x)  £  X  :  x  +  (—x)  =  0 

lx  =  x  \/x  £  X 

a(/3x)  =  (a(3)x  Va,/3eE  \/x  £  X 
{a  +  (3)x  =  ax  + /3x  Va,/3eR  \/x  £  X 
a(x  +  y)  =  ax  +  ay  Va  £  1R  \/x,y£X. 

The  dimension  of  the  space  dim(A’)  is  the  maximum  number  of  linearly  inde¬ 
pendent  vectors  x  in  the  space.  A  set  {x\,  x2, . . . ,  xn}  of  linearly  independent 
vectors  is  a  basis  of  vector  space  X ,  and  each  vector  y  in  the  space  can  be 
uniquely  expressed  as  a  linear  combination  of  vectors  from  the  basis 


y  =  ciasi  +  c2x2  +  .  ■ .  +  cnxn,  (A. 24) 

where  the  constants  ci,  c2, . . . ,  cn  are  said  to  be  the  components  of  the  vector 
y  in  the  basis  {xi,x2, . . .  ,xn}. 

A  subset  y  of  a  vector  space  X  is  a  subspace  y  C  X  if  it  is  a  vector  space 
with  the  operations  of  vector  sum  and  product  of  a  scalar  by  a  vector,  i.e., 

ax  +  (3y  £  y  Va,/3£lR.  Wx,y£y. 

According  to  a  geometric  interpretation,  a  subspace  is  a  hyperplane  passing 
by  the  origin  (null  element)  of  X . 

The  scalar  product  <  x,y  >  of  two  vectors  x  and  y  of  dimensions  {in  x 
1)  is  the  scalar  that  is  obtained  by  summing  the  products  of  the  respective 
components  in  a  given  basis 

<  x,  y  >=  xiyi  +  x2y2  +  . . .  +  xmym  =  xTy  =  yTx.  (A. 25) 

Two  vectors  are  said  to  be  orthogonal  when  their  scalar  product  is  null: 

xTy  =  0.  (A. 26) 


The  norm  of  a  vector  can  be  defined  as 


||£C||  =  \/  XTX. 


(A. 27) 
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It  is  possible  to  show  that  both  the  triangle  inequality 


\\x  +  y\\  <  ||a:||  +  ||y|| 


(A. 28) 


and  the  Schwarz  inequality 

\xTy\<\\x\\\\y\\.  (A. 29) 

hold.  A  unit  vector  a;  is  a  vector  whose  norm  is  unity,  i.e.,  xT x  =  1.  Given  a 
vector  *,  its  unit  vector  is  obtained  by  dividing  each  component  by  its  norm: 

*  =  77—77*-  (A. 30) 


A  typical  example  of  vector  space  is  the  Euclidean  space  whose  dimension  is 
3;  in  this  case  a  basis  is  constituted  by  the  unit  vectors  of  a  coordinate  frame. 

The  vector  product  of  two  vectors  x  and  y  in  the  Euclidean  space  is  the 
vector 


*  x  y  = 


*22/3  -  *32/2 
*32/1  -  *12/3 
*12/2  -  *22/1 


(A.31) 


The  following  properties  hold: 


x  x  x  =  0 


x  x  y  =  —y  x  x 

xx{y  +  z)  =  xxy  +  xxz. 

The  vector  product  of  two  vectors  x  and  y  can  be  expressed  also  as  the 
product  of  a  matrix  operator  S{x)  by  the  vector  y.  In  fact,  by  introducing 


the  skew- symmetric  matrix 

0 

*3 

*2 

S(x)  = 

*3 

0 

-*1 

(A. 32) 

_  *2 

*1 

0 

obtained  with  the  components  of 

'  vector 

x,  the  vector  product  x 

x  y  is  given 

by 

x  x  y  = 

=  S(x)y 

=  - 

S{y)x 

(A. 33) 

as  can  be  easily  verified.  Moreover,  the  following  properties  hold: 

S(x)x  =  ST(x)x  =  0 
S{ax  +  (3y)  =  aS(x )  +  /3S(y). 

Given  three  vectors  x,  y ,  z  in  the  Euclidean  space,  the  following  expres¬ 
sions  hold  for  the  scalar  triple  products: 

xT(y  x  z)  =  yT(z  x  x)  =  zT(x  x  y).  (A. 34) 

If  any  two  vectors  of  three  are  equal,  then  the  scalar  triple  product  is  null; 

e-g-, 


xT (x  x  y)  =  0. 
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A. 4  Linear  Transformation 

Consider  a  vector  space  X  of  dimension  n  and  a  vector  space  y  of  dimension 
to  with  to  <  n.  The  linear  transformation  (or  linear  map)  between  the  vectors 
x  £  X  and  y  £  y  can  be  defined  as 

y  =  Ax  (A. 35) 

in  terms  of  the  matrix  A  of  dimensions  (to  x  n).  The  range  space  (or  simply 
range)  of  the  transformation  is  the  subspace 

7 1(A)  =  {y:y  =  Ax,  x  £  X}  C  y,  (A.36) 

which  is  the  subspace  generated  by  the  linearly  independent  columns  of  matrix 
A  taken  as  a  basis  of  y.  It  is  easy  to  recognize  that 

g(A)  =  dim(72.(A)).  (A. 37) 

On  the  other  hand,  the  null  space  (or  simply  null)  of  the  transformation  is 
the  subspace 

A f{A)  =  {x:  Ax  =  0,  x  £  X}  C  X .  (A.38) 

Given  a  matrix  A  of  dimensions  ( m  x  n) ,  the  notable  result  holds: 

g{A)  +  dim(A/"(A))  =  n.  (A. 39) 

Therefore,  if  g(A)  =  r  <  min  {to,  n},  then  dim(7?.(A))  =  r  and  dim(A/"(A))  = 
n  —  r.  It  follows  that  if  to  <  n,  then  Af(A)  0  independently  of  the  rank  of 
A\  if  to  =  n,  then  A f(A)  yf  0  only  in  the  case  of  g(A)  =  r  <  m. 

If  x  £  N(A)  and  y  £  7 Z{Ar),  then  yTx  =  0,  i.e. ,  the  vectors  in  the  null 
space  of  A  are  orthogonal  to  each  vector  in  the  range  space  of  the  transpose 
of  A.  It  can  be  shown  that  the  set  of  vectors  orthogonal  to  each  vector  of 
the  range  space  of  AT  coincides  with  the  null  space  of  A,  whereas  the  set  of 
vectors  orthogonal  to  each  vector  in  the  null  space  of  AT  coincides  with  the 
range  space  of  A.  In  symbols: 

Af(A)  e^-^A71)  TZ(A)=Af±(AT)  (A. 40) 

where  _L  denotes  the  orthogonal  complement  of  a  subspace. 

If  the  matrix  A  in  (A. 35)  is  square  and  idempotent,  the  matrix  represents 
the  projection  of  space  X  into  a  subspace. 

A  linear  transformation  allows  the  definition  of  the  norm  of  a  matrix  A 
induced  by  the  norm  defined  for  a  vector  x  as  follows.  In  view  of  the  property 

l|Au||  <  ||A||  ||*||,  (A. 41) 


the  norm  of  A  can  be  defined  as 
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which  can  also  be  computed  as 


max  II Acc II. 
11*11=1 

A  direct  consequence  of  (A. 41)  is  the  property 

|| AB||  <  || A||  ||B| 


A  different  norm  of  a  matrix  is  the  Frobenius  norm  defined  as 

\\A\\f=  (Tr(ATA))V2 


(A. 43) 


(A. 44) 


A. 5  Eigenvalues  and  Eigenvectors 

Consider  the  linear  transformation  on  a  vector  u  established  by  an  (n  x  ?r) 
square  matrix  A.  If  the  vector  resulting  from  the  transformation  has  the  same 
direction  of  u  (with  u  ^  0),  then 


Au  =  Xu.  (A. 45) 

The  equation  in  (A. 45)  can  be  rewritten  in  matrix  form  as 

(A7  —  A)u  =  0.  (A. 46) 

For  the  homogeneous  system  of  equations  in  (A. 46)  to  have  a  solution  different 
from  the  trivial  one  u  =  0,  it  must  be 

det(A7  -  A)  =  0  (A.47) 

which  is  termed  a  characteristic  equation.  Its  solutions  Ai,...,An  are  the 
eigenvalues  of  matrix  A;  they  coincide  with  the  eigenvalues  of  matrix  A1 .  On 
the  assumption  of  distinct  eigenvalues,  the  n  vectors  Ui  satisfying  the  equation 

(A^7  —  A)ui  =  0  i  =  l,...,n  (A. 48) 

are  said  to  be  the  eigenvectors  associated  with  the  eigenvalues  A,;. 

The  matrix  U  formed  by  the  column  vectors  it,;  is  invertible  and  constitutes 
a  basis  in  the  space  of  dimension  n.  Further,  the  similarity  transformation 
established  by  U 

A  =  U~1AU  (A. 49) 

is  so  that  A  =  diag{Ai, . . . ,  A„}.  It  follows  that  det(A)  =  Y\a=i 

If  the  matrix  A  is  symmetric ,  its  eigenvalues  are  real  and  A  can  be  written 
as 

A  =  UtAU ;  (A. 50) 

hence,  the  eigenvector  matrix  U  is  orthogonal. 
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A. 6  Bilinear  Forms  and  Quadratic  Forms 

A  bilinear  form  in  the  variables  x*  and  is  the  scalar 

m  n 

B  =  aijXiVo 

i=l  j= 1 

which  can  be  written  in  matrix  form 

B(x ,  y)  =  x1  Ay  =  yT  A1  x  (A. 51) 

where  x=[x\  x2  ...  xm  ]T,  y  =  [ y1  y2  ■  ■  ■  yn  ]T,  and  A  is  the  (to  x 
n)  matrix  of  the  coefficients  at]  representing  the  core  of  the  form. 

A  special  case  of  bilinear  form  is  the  quadratic  form 

Q(x)  =  xT  Ax  (A. 52) 

where  A  is  an  ( n  x  n)  square  matrix.  Hence,  for  computation  of  (A.  52),  the 
matrix  A  can  be  replaced  with  its  symmetric  part  As  given  by  (A. 6).  It  follows 
that  if  A  is  a  skew-symmetric  matrix,  then 

xT  Ax  =  0  \/x. 

The  quadratic  form  (A. 52)  is  said  to  be  positive  definite  if 

xT Ax  >  0  Vx  0  xT Ax  =  0  x  =  0.  (A. 53) 

The  matrix  A  core  of  the  form  is  also  said  to  be  positive  definite.  Analogously, 
a  quadratic  form  is  said  to  be  negative  definite  if  it  can  be  written  as  —Q(x)  = 
— xT Ax  where  Q(x)  is  positive  definite. 

A  necessary  condition  for  a  square  matrix  to  be  positive  definite  is  that 
its  elements  on  the  diagonal  are  strictly  positive.  Further,  in  view  of  (A. 50), 
the  eigenvalues  of  a  positive  definite  matrix  are  all  positive.  If  the  eigenvalues 
are  not  known,  a  necessary  and  sufficient  condition  for  a  symmetric  matrix  to 
be  positive  definite  is  that  its  principal  minors  are  strictly  positive  ( Sylvester 
criterion).  It  follows  that  a  positive  definite  matrix  is  full-rank  and  thus  it  is 
always  invertible. 

A  symmetric  positive  definite  matrix  A  can  always  be  decomposed  as 

A  =  UtAU  (A.  54) 

where  U  is  an  orthogonal  matrix  of  eigenvectors  ( UTU  =  I)  and  A  is  the 
diagonal  matrix  of  the  eigenvalues  of  A. 

Let  Amin(A)  and  Amax(A)  respectively  denote  the  smallest  and  largest 
eigenvalues  of  a  positive  definite  matrix  A  (Amin,Amax  >  0).  Then,  the 
quadratic  form  in  (A. 52)  satisfies  the  following  inequality: 

Amin(A)||a;||2  <  xT Ax  <  Amax(A)||a:||2. 


(A. 55) 
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An  (n  x  n )  square  matrix  A  is  said  to  be  positive  semi-definite  if 

xT Ax  >  0  Vax  (A. 56) 

This  definition  implies  that  g(A)  =  r  <  n,  and  thus  r  eigenvalues  of  A 
are  positive  and  n  —  r  are  null.  Therefore,  a  positive  semi-definite  matrix  A 
has  a  null  space  of  finite  dimension,  and  specifically  the  form  vanishes  when 
x  £  J\f(A).  A  typical  example  of  a  positive  semi-definite  matrix  is  the  matrix 
A  =  HtH  where  H  is  an  (m  x  n)  matrix  with  m  <  n.  In  an  analogous  way, 
a  negative  semi-definite  matrix  can  be  defined. 

Given  the  bilinear  form  in  (A. 51),  the  gradient  of  the  form  with  respect 
to  x  is  given  by 

VxB(*,y)  =  (®^)T  =  Ay,  (A.57) 

whereas  the  gradient  of  B  with  respect  to  y  is  given  by 

X/yB(x,y)=  (dB^gyy^  =ATx.  (A. 58) 

Given  the  quadratic  form  in  (A. 52)  with  A  symmetric,  the  gradient  of  the 
form  with  respect  to  x  is  given  by 


V  xQ{x) 


f  dQjx) 

V  dx 


T 

=  2  Ax. 


(A. 59) 


Further,  if  x  and  A  are  differentiable  functions  of  t,  then 

Q{x)  =  Q(x(t ))  =  2  xT  Ax  +  xT  Ax\  (A. 60) 

if  A  is  constant,  then  the  second  term  obviously  vanishes. 


A. 7  Pseudo-inverse 

The  inverse  of  a  matrix  can  be  defined  only  when  the  matrix  is  square  and 
nonsingular.  The  inverse  operation  can  be  extended  to  the  case  of  non-square 
matrices.  Consider  a  matrix  A  of  dimensions  (m  x  n)  with  g(A)  =  min{m,  n} 
If  m  <  n,  a  right  inverse  of  A  can  be  defined  as  the  matrix  Ar  of  dimen¬ 
sions  (n  x  m)  so  that 

4  4  —  T 

If  instead  m  >  n,  a  left  inverse  of  A  can  be  defined  as  the  matrix  Ai  of 
dimensions  (n  x  m)  so  that 

AiA  =  I  n. 
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If  A  has  more  columns  than  rows  (in  <  n)  and  has  rank  m,  a  special  right 
inverse  is  the  matrix 

Ai  =  At(AAt)~1  (A. 61) 

which  is  termed  right  pseudo-inverse,  since  AAj,  =  Im.  If  Wr  is  an  ( n  x  n) 
positive  definite  matrix,  a  weighted  right  pseudo-inverse  is  given  by 

At  =  Wf1AT(AWf1AT)-1.  (A. 62) 

If  A  has  more  rows  than  columns  (m  >  n)  and  has  rank  n,  a  special  left 
inverse  is  the  matrix 

A\  =  (AtA)~1At  (A. 63) 

which  is  termed  left,  pseudo-inverse,  since  A\A  =  In.3  If  W i  is  an  (m  x  m) 
positive  definite  matrix,  a  weighted  left  pseudo-inverse  is  given  by 

A]  =  (AtWiA)~1AtWi.  (A. 64) 

The  pseudo-inverse  is  very  useful  to  invert  a  linear  transformation  y  =  Ax 
with  A  a  full-rank  matrix.  If  A  is  a  square  nonsingular  matrix,  then  obviously 
x  =  A~xy  and  then  Aj  =  A(  =  A~x. 

If  A  has  more  columns  than  rows  (m  <  n)  and  has  rank  m,  then  the 
solution  x  for  a  given  y  is  not  unique;  it  can  be  shown  that  the  expression 

x  =  A^y  +  (I  —  A^A)k,  (A. 65) 

with  k  an  arbitrary  (n  x  1)  vector  and  A '  as  in  (A. 61),  is  a  solution  to  the 
system  of  linear  equations  established  by  (A. 35).  The  term  Af  y  G  Af±(A)  = 
1Z(At)  minimizes  the  norm  of  the  solution  ||a:||.  The  term  (I  —  A^  A)k  is  the 
projection  of  k  in  Af(A)  and  is  termed  homogeneous  solution ;  as  k  varies, 
all  the  solutions  to  the  homogeneous  equation  system  Ax  =  0  associated 
with  (A. 35)  are  generated. 

On  the  other  hand,  if  A  has  more  rows  than  columns  (m  >  n),  the  equation 
in  (A. 35)  has  no  solution;  it  can  be  shown  that  an  approximate  solution  is  given 

by 

x  =  A^y  (A. 66) 

where  A'  as  in  (A. 63)  minimizes  || y  —  Ax ||.  If  instead  y  £  7Z(A),  then  (A. 66) 
is  a  real  solution. 

Notice  that  the  use  of  the  weighted  (left  or  right)  pseudo-inverses  in  the 
solution  to  the  linear  equation  systems  leads  to  analogous  results  where  the 
minimized  norms  are  weighted  according  to  the  metrics  defined  by  matrices 
Wr  and  W i,  respectively. 

The  results  of  this  section  can  be  easily  extended  to  the  case  of  (square 
or  nonsquare)  matrices  A  not  having  full-rank.  In  particular,  the  expres¬ 
sion  (A. 66)  (with  the  pseudo-inverse  computed  by  means  of  the  singular  value 
decomposition  of  A)  gives  the  minimum- norm  vector  among  all  those  mini¬ 
mizing  ||  y  —  Ax  || . 

3  Subscripts  l  and  r  are  usually  omitted  whenever  the  use  of  a  left  or  right  pseudo¬ 
inverse  is  clear  from  the  context. 
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A. 8  Singular  Value  Decomposition 

For  a  nonsquare  matrix  it  is  not  possible  to  define  eigenvalues.  An  extension 
of  the  eigenvalue  concept  can  be  obtained  by  singular  values.  Given  a  matrix 
A  of  dimensions  (?n  x  n),  the  matrix  A1  A  has  n  nonnegative  eigenvalues 
Ai  >  \2  >  . . .  >  A„  >  0  (ordered  from  the  largest  to  the  smallest)  which  can 
be  expressed  in  the  form 

A*  =  of  cr i  >  0. 


The  scalars  <Ti  >  02  >  . . .  >  an  >  0  are  said  to  be  the  singular  values  of 
matrix  A.  The  singular  value  decomposition  (SVD)  of  matrix  A  is  given  by 


A  =  UEVt 

(A. 67) 

where  U  is  an  (to  x  to)  orthogonal  matrix 

U  =  [u1  u2  ...  um], 

(A. 68) 

V  is  an  ( n  x  n)  orthogonal  matrix 

V=[V!  v2  ...  Vn] 

(A. 69) 

and  E  is  an  (to  x  n)  matrix 

V=  p  ^  D  =  diag{oi,  ct2,  •  • 

.  ,  (Jr} 

(A. 70) 

where  o\  >  er2  >  . . .  >  ay  >  0.  The  number  of  non- null  singular  values  is 
equal  to  the  rank  r  of  matrix  A. 

The  columns  of  U  are  the  eigenvectors  of  the  matrix  AA1 ,  whereas  the 
columns  of  V  are  the  eigenvectors  of  the  matrix  AT  A.  In  view  of  the  partitions 
of  U  and  V  in  (A. 68),  (A. 69),  it  is  Avt  =  aiUi,  for  i  =  1, . . . ,  r  and  Avi  =  0, 
for  i  =  r  +  1 , . . . ,  n. 

Singular  value  decomposition  is  useful  for  analysis  of  the  linear  transforma¬ 
tion  y  =  Ax  established  in  (A. 35).  According  to  a  geometric  interpretation, 
the  matrix  A  transforms  the  unit  sphere  in  IR™  defined  by  ||a:||  =  1  into  the  set 
of  vectors  y  =  Ax  which  define  an  ellipsoid  of  dimension  r  in  JRm.  The  sin¬ 
gular  values  are  the  lengths  of  the  various  axes  of  the  ellipsoid.  The  condition 
number  of  the  matrix 

o-i 

K  =  - 

(T  y 

is  related  to  the  eccentricity  of  the  ellipsoid  and  provides  a  measure  of 
ill-conditioning  (k  1)  for  numerical  solution  of  the  system  established 
by  (A. 35). 

It  is  worth  noticing  that  the  numerical  procedure  of  singular  value  de¬ 
composition  is  commonly  adopted  to  compute  the  (right  or  left)  pseudo¬ 
inverse  even  in  the  case  of  a  matrix  A  not  having  full  rank.  In  fact, 
from  (A. 67),  (A. 70)  it  is 

A]  =  VS^UT  (A. 71) 
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with 

Df  =  diag  {  — ,  —  —1 .  (A. 72) 

l  al  &  2  0>  J 
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Rigid-body  Mechanics 


The  goal  of  this  appendix  is  to  recall  some  fundamental  concepts  of  rigid  body 
mechanics  which  are  preliminary  to  the  study  of  manipulator  kinematics, 
statics  and  dynamics. 

B.l  Kinematics 

A  rigid  body  is  a  system  characterized  by  the  constraint  that  the  distance 
between  any  two  points  is  always  constant. 

Consider  a  rigid  body  B  moving  with  respect  to  an  orthonormal  reference 
frame  O-xyz  of  unit  vectors  x,  y,  z,  called  fixed  frame.  The  rigidity  assump¬ 
tion  allows  the  introduction  of  an  orthonormal  frame  O'—x'y'z'  attached  to 
the  body,  called  moving  frame ,  with  respect  to  which  the  position  of  any  point 
of  B  is  independent  of  time.  Let  x'(t),  y'(t),  z'(t)  be  the  unit  vectors  of  the 
moving  frame  expressed  in  the  fixed  frame  at  time  t. 

The  orientation  of  the  moving  frame  O'—x'y'z'  at  time  t  with  respect  to 
the  fixed  frame  O-xyz  can  be  expressed  by  means  of  the  orthogonal  (3  x  3) 
matrix 

x'T(t)x  y'T(t)x  zlT(t)x 

R(t)  =  x'T\t)y  y'T\t)y  z'T\t)y  ,  (B.l) 

x'T{t)z  y'T{t)z  z'T(t)z 

which  is  termed  rotation  matrix  defined  in  the  orthonormal  special  group 
SO(3)  of  the  (3  x  3)  matrices  with  orthonormal  columns  and  determinant 
equal  to  1.  The  columns  of  the  matrix  in  (B.l)  represent  the  components 
of  the  unit  vectors  of  the  moving  frame  when  expressed  in  the  fixed  frame, 
whereas  the  rows  represent  the  components  of  the  unit  vectors  of  the  fixed 
frame  when  expressed  in  the  moving  frame. 

Let  p '  be  the  constant  position  vector  of  a  generic  point  P  of  B  in  the 
moving  frame  O'—x'y'z'.  The  motion  of  P  with  respect  to  the  fixed  frame 
O-xyz  is  described  by  the  equation 

P(t)  =  Po'(t)  +  R(t)p', 


(B.2) 
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where  p0/(f)  is  the  position  vector  of  origin  O'  of  the  moving  frame  with 
respect  to  the  fixed  frame. 

Notice  that  a  position  vector  is  a  bound  vector  since  its  line  of  application 
and  point  of  application  are  both  prescribed,  in  addition  to  its  direction;  the 
point  of  application  typically  coincides  with  the  origin  of  a  reference  frame. 
Therefore,  to  transform  a  bound  vector  from  a  frame  to  another,  both  trans¬ 
lation  and  rotation  between  the  two  frames  must  be  taken  into  account. 

If  the  positions  of  the  points  of  B  in  the  moving  frame  are  known,  it  follows 
from  (B.2)  that  the  motion  of  each  point  of  B  with  respect  to  the  fixed  frame 
is  uniquely  determined  once  the  position  of  the  origin  and  the  orientation 
of  the  moving  frame  with  respect  to  the  fixed  frame  are  specified  in  time. 
The  origin  of  the  moving  frame  is  determined  by  three  scalar  functions  of 
time.  Since  the  orthonormality  conditions  impose  six  constraints  on  the  nine 
elements  of  matrix  R(t) ,  the  orientation  of  the  moving  frame  depends  only 
on  three  independent  scalar  functions,  three  being  the  minimum  number  of 
parameters  to  represent  50(3). 1 

Therefore,  a  rigid  body  motion  is  described  by  arbitrarily  specifying  six 
scalar  functions  of  time,  which  describe  the  body  pose  (position  +  orientation). 
The  resulting  rigid  motions  belong  to  the  special  Euclidean  group  SE( 3)  = 
1R3  x  50(3). 

The  expression  in  (B.2)  continues  to  hold  if  the  position  vector  p0,(t)  of 
the  origin  of  the  moving  frame  is  replaced  with  the  position  vector  of  any 
other  point  of  B ,  i.e. , 


P(t)  =  Pq  (t)  +  R(t)  (p'  -  p'Q )  (B.3) 

where  Pq{€)  and  p'q  are  the  position  vectors  of  a  point  Q  of  B  in  the  fixed 
and  moving  frames,  respectively. 

In  the  following,  for  simplicity  of  notation,  the  dependence  on  the  time 
variable  t  will  be  dropped. 

Differentiating  (B.3)  with  respect  to  time  gives  the  known  velocity  com¬ 
position  rule 

P  =  Pq  +  “>  x  (p-pQ),  (B.4) 

where  oj  is  the  angular  velocity  of  rigid  body  B.  Notice  that  u>  is  a  free  vector 
since  its  point  of  application  is  not  prescribed.  To  transform  a  free  vector  from 
a  frame  to  another,  only  rotation  between  the  two  frames  must  be  taken  into 
account. 

By  recalling  the  definition  of  the  skew-symmetric  operator  S(-)  in  (A. 32), 
the  expression  in  (B.4)  can  be  rewritten  as 

P  =  Pq  +  S(u)(p  -  pQ) 

=  pQ  +  S(uj)R(p'  -  p'Q). 


1  The  minimum  number  of  parameters  represent  a  special  orthonormal 
group  SO(m)  is  equal  to  m(m  —  l)/2. 
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Comparing  this  equation  with  the  formal  time  derivative  of  (B.3)  leads  to  the 
result 

R=S(u)R.  (B.5) 

In  view  of  (B.4),  the  elementary  displacement  of  a  point  P  of  the  rigid  body 
B  in  the  time  interval  ( t ,  t  +  dt)  is 

dp  =  pdt  =  (pQ  +  uj  x  (p  -  pQ))dt  (B.6) 

=  dpQ  +  udtx  (p-pQ). 

Differentiating  (B.4)  with  respect  to  time  yields  the  following  expression 
for  acceleration: 

p  =  pQ  +  u  x  (p-pQ)  +  uj  x  (ujx  (p-  pQ )) .  (B.7) 


B.2  Dynamics 


Let  pdV  be  the  mass  of  an  elementary  particle  of  a  rigid  body  B ,  where  p 
denotes  the  density  of  the  particle  of  volume  dV .  Also  let  Vg  be  the  body 
volume  and  to  =  fv  pdV  its  total  mass  assumed  to  be  constant.  If  p  denotes 
the  position  vector  of  the  particle  of  mass  pdV  in  the  frame  O-xyz,  the  centre 
of  mass  of  B  is  defined  as  the  point  C  whose  position  vector  is 

Pc=f~  [  PPdV-  (B-8) 

m  JvB 

In  the  case  when  B  is  the  union  of  n  distinct  parts  of  mass  mi,  •  •  • ,  to„  and 
centres  of  mass  pci  . . .  Pcni  the  centre  of  mass  of  B  can  be  computed  as 


Pc  =  Y]  rn.iPa 


with  to  =  mi- 

Let  r  be  a  line  passing  by  O  and  d{p)  the  distance  from  r  of  the  particle 
of  B  of  mass  pdV  and  position  vector  p.  The  moment  of  inertia  of  body  B 
with  respect  to  line  r  is  defined  as  the  positive  scalar 

/,  =  /  d2(p)pdV . 

JvB 

Let  r  denote  the  unit  vector  of  line  r;  then,  the  moment  of  inertia  of  B  with 
respect  to  line  r  can  be  expressed  as 

Ir  =  rT  (^  j  ST (p)S(p)pdV^j  r  =  rTI0r , 


(B.9) 
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where  S(-)  is  the  skew-symmetric  operator  in  (A. 31),  and  the  symmetric , 
positive  definite  matrix 

'JvM+p^PdV  ~SvB  PxPypdV  ■lvB  PxPzpdV  1 

lo  =  *  fvB  (Px  +  Pz)pdV  -  JVb  PyPzpdV 

*  *  fvB(Pl+Pl)PdV- 

POxx  1 Oxy  lo  xz 

—  *  lOyy  —lOyz  (B.10) 

*  *  lOzz 

is  termed  inertia  tensor  of  body  B  relative  to  pole  O.2  The  (positive)  elements 
Ioxx ,  loyyi  lozz  are  the  inertia  moments  with  respect  to  three  coordinate  axes 
of  the  reference  frame,  whereas  the  elements  Ioxy ,  pOxz >  loyz  (of  any  sign) 
are  said  to  be  products  of  inertia. 

The  expression  of  the  inertia  tensor  of  a  rigid  body  B  depends  both  on  the 
pole  and  the  reference  frame.  If  orientation  of  the  reference  frame  with  origin 
at  O  is  changed  according  to  a  rotation  matrix  R ,  the  inertia  tensor  l'0  in 
the  new  frame  is  related  to  Io  by  the  relationship 

Io  =  RI'0RT  (B.ll) 

The  way  an  inertia  tensor  is  transformed  when  the  pole  is  changed  can  be 
inferred  by  the  following  equation,  also  known  as  Steiner  theorem  or  parallel 
axis  theorem: 

Io  =  Ic  +  mST(pc)S(pc),  (B.12) 

where  Ic  is  the  inertia  tensor  relative  to  the  centre  of  mass  of  B ,  when  ex¬ 
pressed  in  a  frame  parallel  to  the  frame  with  origin  at  O  and  with  origin  at 
the  centre  of  mass  C. 

Since  the  inertia  tensor  is  a  symmetric  positive  definite  matrix,  there  al¬ 
ways  exists  a  reference  frame  in  which  the  inertia  tensor  attains  a  diagonal 
form;  such  a  frame  is  said  to  be  a  principal  frame  (relative  to  pole  O)  and 
its  coordinate  axes  are  said  to  be  principal  axes.  In  the  case  when  pole  O 
coincides  with  the  centre  of  mass,  the  frame  is  said  to  be  a  central  frame  and 
its  axes  are  said  to  be  central  axes. 

Notice  that  if  the  rigid  body  is  moving  with  respect  to  the  reference  frame 
with  origin  at  O ,  then  the  elements  of  the  inertia  tensor  Io  become  a  func¬ 
tion  of  time.  With  respect  to  a  pole  and  a  reference  frame  attached  to  the 
body  (moving  frame),  instead,  the  elements  of  the  inertia  tensor  represent  six 
structural  constants  of  the  body  which  are  known  once  the  pole  and  reference 
frame  have  been  specified. 


2  The  symbol  V  has  been  used  to  avoid  rewriting  the  symmetric  elements. 
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Let  p  be  the  velocity  of  a  particle  of  B  of  elementary  mass  pdV  in  frame 
O-xyz.  The  linear  momentum  of  body  B  is  defined  as  the  vector 


l  = 


/  ppdV  =  rnpc . 

JvB 


(B.13) 


Let  17  be  any  point  in  space  and  p^  its  position  vector  in  frame  O-xyz ; 
then,  the  angular  momentum  of  body  B  relative  to  pole  17  is  defined  as  the 
vector 


kn=  px(pa~  p)pdV . 

JVn 


'Vt3 

The  pole  can  be  either  fixed  or  moving  with  respect  to  the  reference  frame. 
The  angular  momentum  of  a  rigid  body  has  the  following  notable  expression: 


kn  =  I cw  +  mpc  x  (pn  -  pc), 


(B.14) 


where  Ic  is  the  inertia  tensor  relative  to  the  centre  of  mass,  when  expressed 
in  a  frame  parallel  to  the  reference  frame  with  origin  at  the  centre  of  mass. 

The  forces  acting  on  a  generic  system  of  material  particles  can  be  distin¬ 
guished  into  internal  forces  and  external  forces. 

The  internal  forces,  exerted  by  one  part  of  the  system  on  another,  have 
null  linear  and  angular  momentum  and  thus  they  do  not  influence  rigid  body 
motion. 

The  external  forces,  exerted  on  the  system  by  an  agency  outside  the  sys¬ 
tem,  in  the  case  of  a  rigid  body  B  are  distinguished  into  active  forces  and 
reaction  forces. 

The  active  forces  can  be  either  concentrated  forces  or  body  forces.  The 
former  are  applied  to  specific  points  of  B ,  whereas  the  latter  act  on  all  ele¬ 
mentary  particles  of  the  body.  An  example  of  body  force  is  the  gravitational 
force  which,  for  any  elementary  particle  of  mass  pdV,  is  equal  to  g0pdV  where 
g0  is  the  gravity  acceleration  vector. 

The  reaction  forces  are  those  exerted  because  of  surface  contact  between 
two  or  more  bodies.  Such  forces  can  be  distributed  on  the  contact  surfaces  or 
they  can  be  assumed  to  be  concentrated. 

For  a  rigid  body  B  subject  to  gravitational  force,  as  well  as  to  active  and 
or  reaction  forces  / 1  •  •  •  fn  concentrated  at  points  jq  . . .  pn ,  the  resultant  of 
the  external  forces  /  and  the  resultant  moment  pn  with  respect  to  a  pole  17 
are  respectively 


f  =  9oPdv  +  f  ‘  =  m9o  +  fi 


(B.15) 


>VB 


Po  =  9o  x  (Po  -  p)pdV  +  f  i  x  (Pn  ^  Pi) 


IV B 


=  mg0  X  (pn  -  Pc) +  J2  fi  x  “  Pi)- 


(B.16) 
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In  the  case  when  /  and  p, q  are  known  and  it  is  desired  to  compute  the 
resultant  moment  with  respect  to  a  point  ft'  other  than  I?,  the  following 
relation  holds: 

Mn-  =  +  f  x  {pn>  ~Pn)-  (B-17) 

Consider  now  a  generic  system  of  material  particles  subject  to  external 
forces  of  resultant  /  and  resultant  moment  Pq.  The  motion  of  the  system 
in  a  frame  O-xyz  is  established  by  the  following  fundamental  principles  of 
dynamics  (Newton  laws  of  motion): 

/  =  l  (B.18) 

Hn  =  kn  (B.19) 

where  C  is  a  pole  fixed  or  coincident  with  the  centre  of  mass  C  of  the  system. 
These  equations  hold  for  any  mechanical  system  and  can  be  used  even  in  the 
case  of  variable  mass.  For  a  system  with  constant  mass,  computing  the  time 
derivative  of  the  momentum  in  (B.18)  gives  Newton  equations  of  motion  in 
the  form 

/  =  mpc ,  (B.20) 

where  the  quantity  on  the  right-hand  side  represents  the  resultant  of  inertia 
forces. 

If,  besides  the  assumption  of  constant  mass,  the  assumption  of  rigid  system 
holds  too,  the  expression  in  (B.14)  of  the  angular  momentum  with  (B.19)  yield 
Euler  equations  of  motion  in  the  form 

Hn  =  Indo  +  w  x  {Iquj),  (B.21) 

where  the  quantity  on  the  right-hand  side  represents  the  resultant  moment  of 
inertia  forces. 

For  a  system  constituted  by  a  set  of  rigid  bodies,  the  external  forces  obvi¬ 
ously  do  not  include  the  reaction  forces  exerted  between  the  bodies  belonging 
to  the  same  system. 


B.3  Work  and  Energy 

Given  a  force  f,  applied  at  a  point  of  position  pt  with  respect  to  frame  O-xyz, 
the  elementary  work  of  the  force  /,  on  the  displacement  dpt  =  ptdt  is  defined 
as  the  scalar 

dWi  =  fjdpi . 

For  a  rigid  body  B  subject  to  a  system  of  forces  of  resultant  /  and  resultant 
moment  p,Q  with  respect  to  any  point  Q  of  B,  the  elementary  work  on  the 
rigid  displacement  (B.6)  is  given  by 

dW  =  ( fTpQ  +  PQio)dt  =  fT  dp  Q  +  p^Lodt. 


(B.22) 
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The  kinetic  energy  of  a  body  B  is  defined  as  the  scalar  quantity 

T  =  \  [  pTppdV 
z  JvB 

which,  for  a  rigid  body,  takes  on  the  notable  expression 

T  =  T^mpcPc  +  ^uTIcu>  (B.23) 

where  Ic  is  the  inertia  tensor  relative  to  the  centre  of  mass  expressed  in  a 
frame  parallel  to  the  reference  frame  with  origin  at  the  centre  of  mass. 

A  system  of  position  forces,  i.e.,  the  forces  depending  only  on  the  positions 
of  the  points  of  application,  is  said  to  be  conservative  if  the  work  done  by  each 
force  is  independent  of  the  trajectory  described  by  the  point  of  application  of 
the  force  but  it  depends  only  on  the  initial  and  final  positions  of  the  point  of 
application.  In  this  case,  the  elementary  work  of  the  system  of  forces  is  equal 
to  minus  the  total  differential  of  a  scalar  function  termed  potential  energy , 
i.e., 

dW  =  - dU .  (B.24) 

An  example  of  a  conservative  system  of  forces  on  a  rigid  body  is  the  gravita¬ 
tional  force,  with  which  is  associated  the  potential  energy 

U  =  ~  [  QoPpdV  =  - mglpc .  (B.25) 

JV e 


B.4  Constrained  Systems 

Consider  a  system  Br  of  r  rigid  bodies  and  assume  that  all  the  elements  of  Br 
can  reach  any  position  in  space.  In  order  to  find  uniquely  the  position  of  all  the 
points  of  the  system,  it  is  necessary  to  assign  a  vector  x  =  [x\  ...  xp]T 

of  6r  =  p  parameters,  termed  configuration.  These  parameters  are  termed 
Lagrange  or  generalized  coordinates  of  the  unconstrained  system  Br,  and  p 
determines  the  number  of  degrees  of  freedom  (DOFs). 

Any  limitation  on  the  mobility  of  the  system  Br  is  termed  constraint.  A 
constraint  acting  on  Br  is  said  to  be  holonomic  if  it  is  expressed  by  a  system 
of  equations 

h(x,t)  =  0,  (B.26) 

where  h  is  a  vector  of  dimensions  (s  x  1),  with  s  <  in.  On  the  other  hand, 
a  constraint  in  the  form  h(x,x,t)  =  0  which  is  nonintegrable  is  said  to 
be  nonholonomic.  For  simplicity,  only  equality  (or  bilateral )  constraints  are 
considered.  If  the  equations  in  (B.26)  do  not  explicitly  depend  on  time,  the 
constraint  is  said  to  be  scleronomic. 

On  the  assumption  that  h  has  continuous  and  continuously  differentiable 
components,  and  its  Jacobian  dh/dx  has  full  rank,  the  equations  in  (B.26) 
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allow  the  elimination  of  s  out  of  to  coordinates  of  the  system  Br .  With  the 
remaining  n  =  m  —  s  coordinates  it  is  possible  to  determine  uniquely  the 
configurations  of  Br  satisfying  the  constraints  (B.26).  Such  coordinates  are 
the  Lagrange  or  generalized  coordinates  and  n  is  the  number  of  degrees  of 
freedom  of  the  unconstrained  system  Br. 

The  motion  of  a  system  Br  with  n  DOFs  and  holonomic  equality  con¬ 
straints  can  be  described  by  equations  of  the  form 


x  =  x(q(t),t), 


(B.27) 


where  q(t)  =  [qi(t)  ...  qn(t)  }T  is  a  vector  of  Lagrange  coordinates. 

The  elementary  displacement  of  system  (B.27)  relative  to  the  interval  (t,  t+ 
dt)  is  defined  as 


dx  = 


dx(q,t) 

dq 


qdt  + 


dx(q,t) 

dt 


dt. 


(B.28) 


The  virtual  displacement  of  system  (B.27)  at  time  t,  relative  to  an  increment 
(5 A,  is  defined  as  the  quantity 


Sx  = 


dx(q,t) 

dq 


Sq. 


(B.29) 


The  difference  between  the  elementary  displacement  and  the  virtual  displace¬ 
ment  is  that  the  former  is  relative  to  an  actual  motion  of  the  system  in  an 
interval  (t,  t  +  dt)  which  is  consistent  with  the  constraints,  while  the  latter  is 
relative  to  an  imaginary  motion  of  the  system  when  the  constraints  are  made 
invariant  and  equal  to  those  at  time  t. 

For  a  system  with  time-invariant  constraints,  the  equations  of  motion 
(B.27)  become 

x  =  x(q(t)),  (B.30) 

and  then,  by  setting  SX  =  dX  =  Xdt ,  the  virtual  displacements  (B.29)  coincide 
with  the  elementary  displacements  (B.28). 

To  the  concept  of  virtual  displacement  can  be  associated  that  of  virtual 
work  of  a  system  of  forces,  by  considering  a  virtual  displacement  instead  of 
an  elementary  displacement. 

If  external  forces  are  distinguished  into  active  forces  and  reaction  forces,  a 
direct  consequence  of  the  principles  of  dynamics  (B.18),  (B.19)  applied  to  the 
system  of  rigid  bodies  Br  is  that,  for  each  virtual  displacement,  the  following 
relation  holds: 

SWm  +  <5Wa  +  SWh  =  0,  (B.31) 

where  SWm,  SWa,  SWh  are  the  total  virtual  works  done  by  the  inertia,  active, 
reaction  forces,  respectively. 

3  In  general,  the  Lagrange  coordinates  of  a  constrained  system  have  a  local  validity; 
in  certain  cases,  such  as  the  joint  variables  of  a  manipulator,  they  can  have  a  global 
validity. 
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In  the  case  of  frictionless  equality  constraints,  reaction  forces  are  exerted 
orthogonally  to  the  contact  surfaces  and  the  virtual  work  is  always  null.  Hence, 
(B.31)  reduces  to 

SWm  +  SWa  =  0.  (B.32) 

For  a  steady  system,  inertia  forces  are  identically  null.  Then  the  condition 
for  the  equilibrium  of  system  £>,,  is  that  the  virtual  work  of  the  active  forces 
is  identically  null  on  any  virtual  displacement,  which  gives  the  fundamental 
equation  of  statics  of  a  constrained  system 


SWa  =  0  (B.33) 

known  as  principle  of  virtual  work.  Expressing  (B.33)  in  terms  of  the  incre¬ 
ment  S\  of  generalized  coordinates  leads  to 

SWa  =  CT$q  =  0  (B.34) 


where  £  denotes  the  (n  x  1)  vector  of  active  generalized  forces. 

In  the  dynamic  case,  it  is  worth  distinguishing  active  forces  into  conserva¬ 
tive  (that  can  be  derived  from  a  potential)  and  nonconservative.  The  virtual 
work  of  conservative  forces  is  given  by 

Bid 

SWC  =  (B.35) 

where  U( A)  is  the  total  potential  energy  of  the  system.  The  work  of  noncon¬ 
servative  forces  can  be  expressed  in  the  form 

SWn  c  =  £TSq,  (B.36) 


where  £  denotes  the  vector  of  nonconservative  generalized  forces.  It  follows 
that  the  vector  of  active  generalized  forces  is 


<  =  (- 


(B.37) 


Moreover,  the  work  of  inertia  forces  can  be  computed  from  the  total  kinetic 
energy  of  system  T  as 


SWm 


BT  _  d_  dT\ 
d q  ~  JtM )  dq' 


(B.38) 


Substituting  (B.35),  (B.36),  (B.38)  into  (B.32)  and  observing  that  (B.32)  holds 
true  for  any  increment  S A  leads  to  Lagrange  equations 


d_  (d£\T 

dt  \  Bq  ) 


(B.39) 


where 


C  =  T  —  U 


(B.40) 
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is  the  Lagrangian  function  of  the  system.  The  equations  in  (B.39)  completely 
describe  the  dynamic  behaviour  of  an  n-DOF  system  with  holonomic  equality 
constraints. 

The  sum  of  kinetic  and  potential  energy  of  a  system  with  time-invariant 
constraints  is  termed  Hamiltonian  function 

U  =  T  +  U.  (B.41) 

Conservation  of  energy  dictates  that  the  time  derivative  of  the  Hamiltonian 
must  balance  the  power  generated  by  the  nonconservative  forces  acting  on  the 

iTq-  (B.42) 

in  (B.42)  becomes 

CTq.  (B.43) 


dH  _ 
dt 

In  view  of  (B.37),  (B.41),  the  equation 


dT 

dt 
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Feedback  Control 


As  a  premise  to  the  study  of  manipulator  decentralized  control  and  centralized 
control,  the  fundamental  principles  of  feedback  control  of  linear  systems  are 
recalled,  and  an  approach  to  the  determination  of  control  laws  for  nonlinear 
systems  based  on  the  use  of  Lyapunov  functions  is  presented. 


C.l  Control  of  Single-input/Single-output  Linear 
Systems 

According  to  classical  automatic  control  theory  of  linear  time-invariant  single¬ 
input /single- output  systems ,  in  order  to  servo  the  output  y(t)  of  a  system  to 
a  reference  r(t),  it  is  worth  adopting  a  negative  feedback  control  structure. 
This  structure  indeed  allows  the  use  of  approximate  mathematical  models  to 
describe  the  input/output  relationship  of  the  system  to  control,  since  negative 
feedback  has  a  potential  for  reducing  the  effects  of  system  parameter  variations 
and  nonmeasurable  disturbance  inputs  d(t)  on  the  output. 

This  structure  can  be  represented  in  the  domain  of  complex  variable  s  as  in 
the  block  scheme  of  Fig.  C.l,  where  G(s),  H(s)  and  C(s )  are  the  transfer  func¬ 
tions  of  the  system  to  control,  the  transducer  and  the  controller,  respectively. 
From  this  scheme  it  is  easy  to  derive 

Y(s)  =  W  (s)R(s)  +  Wd(s)D(s), 

where 

W(s)  =  C{S)G{S) 

1  ’  1  +  C(s)G(s)H(s) 

is  the  closed-loop  input/output  transfer  function  and 

Wd ^  =  1  +  C(s)G(s)H(s) 
is  the  disturbance/output  transfer  function. 


(C.l) 

(C.2) 

(C.3) 
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Fig.  C.l.  Feedback  control  structure 


The  goal  of  the  controller  design  is  to  find  a  control  structure  C(s)  ensuring 
that  the  output  variable  Y"(s)  tracks  a  reference  input  R(s).  Further,  the 
controller  should  guarantee  that  the  effects  of  the  disturbance  input  D(s )  on 
the  output  variable  are  suitably  reduced.  The  goal  is  then  twofold,  namely, 
reference  tracking  and  disturbance  rejection. 

The  basic  problem  for  controller  design  consists  of  the  determination  of  an 
action  C(s)  which  can  make  the  system  asymptotically  stable.  In  the  absence 
of  positive  or  null  real  part  pole/zero  and  zero/pole  cancellation  in  the  open- 
loop  function  F(s)  =  C(s)G(s)H(s),  a  necessary  and  sufficient  condition  for 
asymptotic  stability  is  that  the  poles  of  W(s)  and  IFd(s)  have  all  negative 
real  parts;  such  poles  coincide  with  the  zeros  of  the  rational  transfer  function 
1  +  F(s).  Testing  for  this  condition  can  be  performed  by  resorting  to  stability 
criteria,  thus  avoiding  computation  of  the  function  zeros. 

Routh  criterion  allows  the  determination  of  the  sign  of  the  real  parts  of 
the  zeros  of  the  function  1  +  F(s)  by  constructing  a  table  with  the  coefficients 
of  the  polynomial  at  the  numerator  of  1  +  F(s)  ( characteristic  polynomial). 

Routh  criterion  is  easy  to  apply  for  testing  stability  of  a  feedback  system, 
but  it  does  not  provide  a  direct  relationship  between  the  open-loop  function 
and  stability  of  the  closed-loop  system.  It  is  then  worth  resorting  to  Nyquist 
criterion  which  is  based  on  the  representation,  in  the  complex  plane,  of  the 
open-loop  transfer  function  F(s)  evaluated  in  the  domain  of  real  angular  fre¬ 
quency  (s  =  jco,  —oo  <  oj  <  +00). 

Drawing  of  Nyquist  plot  and  computation  of  the  number  of  circles  made  by 
the  vector  representing  the  complex  number  1  +  F(jui)  when  to  continuously 
varies  from  —00  to  +00  allows  a  test  on  whether  or  not  the  closed-loop  system 
is  asymptotically  stable.  It  is  also  possible  to  determine  the  number  of  positive, 
null  and  negative  real  part  roots  of  the  characteristic  polynomial,  similarly  to 
application  of  Routh  criterion.  Nonetheless,  Nyquist  criterion  is  based  on  the 
plot  of  the  open-loop  transfer  function,  and  thus  it  allows  the  determination  of 
a  direct  relationship  between  this  function  and  closed-loop  system  stability.  It 
is  then  possible  from  an  examination  of  the  Nyquist  plot  to  draw  suggestions 
on  the  controller  structure  C(s)  which  ensures  closed-loop  system  asymptotic 
stability. 
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If  the  closed-loop  system  is  asymptotically  stable,  the  steady-state  response 
to  a  sinusoidal  input  r(t),  with  d(t)  =  0,  is  sinusoidal,  too.  In  this  case,  the 
function  W(s),  evaluated  for  s  =  ju>,  is  termed  frequency  response  function ; 
the  frequency  response  function  of  a  feedback  system  can  be  assimilated  to 
that  of  a  low-pass  filter  with  the  possible  occurrence  of  a  resonance  peak  inside 
its  bandwidth. 

As  regards  the  transducer,  this  should  be  chosen  so  that  its  bandwidth 
is  much  greater  than  the  feedback  system  bandwidth,  in  order  to  ensure 
a  nearly  instantaneous  response  for  any  value  of  ui  inside  the  bandwidth 
of  W  {juj).  Therefore,  setting  H(juj)  «  Hq  and  assuming  that  the  loop  gain 
\C(juj)G(juj)Ho\  1  in  the  same  bandwidth,  the  expression  in  (C.l)  for 
s  =  jto  can  be  approximated  as 


y  (M 


R{ju)  Djjuj) 

H0  C(jcj)H0 


Assuming  R{ju>)  =  H0Yd(jaj)  leads  to 


Y  {ju>)  «  Yd(ju)  + 


D(jv) 

C(ju)H0' 


(C.4) 


i.e. ,  the  output  tracks  the  desired  output  Yd(ju>)  and  the  frequency  compo¬ 
nents  of  the  disturbance  in  the  bandwidth  of  W ( jut )  produce  an  effect  on  the 
output  which  can  be  reduced  by  increasing  \C{ju)Ho\.  Furthermore,  if  the 
disturbance  input  is  a  constant,  the  steady-state  output  is  not  influenced  by 
the  disturbance  as  long  as  C(s )  has  at  least  a  pole  at  the  origin. 

Therefore,  a  feedback  control  system  is  capable  of  establishing  a  propor¬ 
tional  relationship  between  the  desired  output  and  the  actual  output,  as  evi¬ 
denced  by  (C.4).  This  equation,  however,  requires  that  the  frequency  content 
of  the  input  (desired  output)  be  inside  the  frequency  range  for  which  the  loop 
gain  is  much  greater  than  unity. 

The  previous  considerations  show  the  advantage  of  including  a  proportional 
action  and  an  integral  action  in  the  controller  C(s),  leading  to  the  transfer 
function 

C(s)  =  Kr  1  +  sTl  (C.5) 

s 

of  a  proportional-integral  controller  (PI);  Tj  is  the  time  constant  of  the  integral 
action  and  the  quantity  KjTj  is  called  proportional  sensitivity. 

The  adoption  of  a  PI  controller  is  effective  for  low-frequency  response  of 
the  system,  but  it  may  involve  a  reduction  of  stability  margins  and/or  a  reduc¬ 
tion  of  closed- loop  system  bandwidth.  To  avoid  these  drawbacks,  a  derivative 
action  can  be  added  to  the  proportional  and  integral  actions,  leading  to  the 
transfer  function 


C(s)  =  Kt 


1  +  sTi  +  s2TdT / 


(C.6) 


of  a  proportional-integral-derivative  controller  (PID);  To  denotes  the  time 
constant  of  the  derivative  action.  Notice  that  physical  realizability  of  (C.6) 
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demands  the  introduction  of  a  high-frequency  pole  which  little  influences  the 
input/output  relationship  in  the  system  bandwidth.  The  transfer  function 
in  (C.6)  is  characterized  by  the  presence  of  two  zeros  which  provide  a  stabi¬ 
lizing  action  and  an  enlargement  of  the  closed-loop  system  bandwidth.  Band¬ 
width  enlargement  implies  shorter  response  time  of  the  system,  in  terms  of 
both  variations  of  the  reference  signal  and  recovery  action  of  the  feedback 
system  to  output  variations  induced  by  the  disturbance  input. 

The  parameters  of  the  adopted  control  structure  should  be  chosen  so  as 
to  satisfy  requirements  on  the  system  behaviour  at  steady  state  and  during 
the  transient.  Classical  tools  to  determine  such  parameters  are  the  root  locus 
in  the  domain  of  the  complex  variable  s  or  the  Nichols  chart  in  the  domain 
of  the  real  angular  frequency  u>.  The  two  tools  are  conceptually  equivalent. 
Their  potential  is  different  in  that  root  locus  allows  a  control  law  to  be  found 
which  assigns  the  exact  parameters  of  the  closed-loop  system  time  response, 
whereas  Nichols  chart  allows  a  controller  to  be  specified  which  confers  good 
transient  and  steady-state  behaviour  to  the  system  response. 

A  feedback  system  with  strict  requirements  on  the  steady-state  and  tran¬ 
sient  behaviour,  typically,  has  a  response  that  can  be  assimilated  to  that  of  a 
second-order  system.  In  fact,  even  for  closed-loop  functions  of  greater  order, 
it  is  possible  to  identify  a  pair  of  complex  conjugate  poles  whose  real  part 
absolute  value  is  smaller  than  the  real  part  absolute  values  of  the  other  poles. 
Such  a  pair  of  poles  is  dominant  in  that  its  contribution  to  the  transient  re¬ 
sponse  prevails  over  that  of  the  other  poles.  It  is  then  possible  to  approximate 
the  input/output  relationship  with  the  transfer  function 


W{s) 


kw 


M n 


(C.7) 


which  has  to  be  realized  by  a  proper  choice  of  the  controller.  Regarding 
the  values  to  assign  to  the  parameters  characterizing  the  transfer  function 
in  (C.7),  the  following  remarks  are  in  order.  The  constant  kw  represents  the 
input/output  steady-state  gain,  which  is  equal  to  1/Hq  if  C(s)G(s)Hq  has  at 
least  a  pole  at  the  origin.  The  natural  frequency  ajn  is  the  modulus  of  the 
complex  conjugate  poles,  whose  real  part  is  given  by  —C,L0n  where  C,  is  the 
damping  ratio  of  the  pair  of  poles. 

The  influence  of  parameters  £  and  ton  on  the  closed-loop  frequency  re¬ 
sponse  can  be  evaluated  in  terms  of  the  resonance  peak  magnitude 


Mr 


1 

zcTw1’ 


occurring  at  the  resonant  frequency 


LUr  —  LUfi  "\/l 
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Dc(s) 


Fig.  C.2.  Feedback  control  structure  with  feedforward  compensation 


and  of  the  3  dB  bandwidth 

W3  =  w„^/i-2C2  +  \/2  -  4C2  +  4C1. 

A  step  input  is  typically  used  to  characterize  the  transient  response  in  the 
time  domain.  The  influence  of  parameters  £  and  uin  on  the  step  response  can 
be  evaluated  in  terms  of  the  percentage  of  overshoot 

s%  =  100exp(— 7rC/\/l  —  (2), 


of  the  rise  time 


and  of  the  settling  time  within  1% 

ts 


1.8 

Wn 

4.6 

C^n 


The  adoption  of  a  feedforward  compensation  action  represents  a  feasible 
solution  both  for  tracking  a  time-varying  reference  input  and  for  enhancing 
rejection  of  the  effects  of  a  disturbance  on  the  output.  Consider  the  general 
scheme  in  Fig.  C.2.  Let  R(s)  denote  a  given  input  reference  and  Dc(s)  de¬ 
note  a  computed  estimate  of  the  disturbance  D(s);  the  introduction  of  the 
feedforward  action  yields  the  input/output  relationship 


Y(s)  = 


C(s)G(s) 


F(s)G(s) 


1  +  C(s)G(s)H(s) 

,  G(s) 

1  +  C(s)G(s)H(s) 


1  +  C(s)G(s)H(s) 
(D(s)-Dc(s)). 


R(s)  (C.8) 


By  assuming  that  the  desired  output  is  related  to  the  reference  input  by  a 
constant  factor  K,j  and  regarding  the  transducer  as  an  instantaneous  system 
(ff(s)  «  H0  =  1  /Kd)  for  the  current  operating  conditions,  the  choice 


F(s) 


Kd 

G(s) 


(C.9) 
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Fig.  C.3.  Feedback  control  structure  with  inverse  model  technique 


yields  the  input/output  relationship 

r(»)  =  W  +  1  +  c°(;J(a)HoW-oe(»y  (C.10) 

If  \C(juj)G(joj)Ho\  1,  the  effect  of  the  disturbance  on  the  output  is  further 
reduced  by  means  of  an  accurate  estimate  of  the  disturbance. 

Feedforward  compensation  technique  may  lead  to  a  solution,  termed  in¬ 
verse  model  control ,  illustrated  in  the  scheme  of  Fig.  C.3.  It  should  be  re¬ 
marked,  however,  that  such  a  solution  is  based  on  dynamics  cancellation, 
and  thus  it  can  be  employed  only  for  a  minimum-phase  system,  i.e.,  a  system 
whose  poles  and  zeros  have  all  strictly  negative  real  parts.  Further,  one  should 
consider  physical  realizability  issues  as  well  as  effects  of  parameter  variations 
which  prevent  perfect  cancellation. 


C.2  Control  of  Nonlinear  Mechanical  Systems 

If  the  system  to  control  does  not  satisfy  the  linearity  property,  the  control 
design  problem  becomes  more  complex.  The  fact  that  a  system  is  qualified 
as  nonlinear ,  whenever  linearity  does  not  hold,  leads  to  understanding  how 
it  is  not  possible  to  resort  to  general  techniques  for  control  design,  but  it  is 
necessary  to  face  the  problem  for  each  class  of  nonlinear  systems  which  can 
be  defined  through  imposition  of  special  properties. 

On  the  above  premise,  the  control  design  problem  of  nonlinear  systems 
described  by  the  dynamic  model 

H(x)x  +  h(x,  x)  =  u  (C.ll) 

is  considered,  where  [xT  xT]T  denotes  the  (2 n  x  1)  state  vector  of  the 
system,  u  is  the  (n  x  1)  input  vector,  H{x)  is  an  (n  x  n )  positive  definite 
(and  thus  invertible)  matrix  depending  on  x ,  and  h(x,x)  is  an  (n  x  1)  vector 
depending  on  state.  Several  mechanical  systems  can  be  reduced  to  this  class, 
including  manipulators  with  rigid  links  and  joints. 

The  control  law  can  be  found  through  a  nonlinear  compensating  action 
obtained  by  choosing  the  following  nonlinear  state  feedback  law  ( inverse  dy¬ 
namics  control): 


u  =  H(x)v  +  h(x ,  x) 


(C.12) 
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where  H(x)  and  h(x)  respectively  denote  the  estimates  of  the  terms  H(x) 
and  h(x),  computed  on  the  basis  of  measures  on  the  system  state,  and  v  is  a 
new  control  input  to  be  defined  later.  In  general,  it  is 

H{x)  =  H{x)  +  AH{x)  (C.13) 

h(x,  x)  =  h(x,  x)  +  Ah(x1  x)  (C.14) 

because  of  the  unavoidable  modelling  approximations  or  as  a  consequence  of 
an  intentional  simplification  in  the  compensating  action.  Substituting  (C.12) 
into  (C.ll)  and  accounting  for  (C.13),  (C.14)  yields 

x  =  v  +  z(x,x,v)  (C.15) 


where 

z(x,x,v)  =  H^1{x)(yAH(x)v  +  Ah(x,  x)) . 


If  tracking  of  a  trajectory  (xd(t),xd(t),Xd{t))  is  desired,  the  tracking  error 
can  be  defined  as 


xd  —  x 
xd-x 


(C.16) 


and  it  is  necessary  to  derive  the  error  dynamics  equation  to  study  convergence 
of  the  actual  state  to  the  desired  one.  To  this  end,  the  choice 


v  =  xd  +  w(e), 


substituted  into  (C.15),  leads  to  the  error  equation 

e  =  Fe  -  Gw{e)  -  Gz(e,  xdl  xdl  xd ), 
where  the  (2 n  x  2 n)  and  (2n  x  n)  matrices,  respectively, 


F  = 


O 

O 


I 

O 


(C.17) 


(C.18) 


follow  from  the  error  definition  in  (C.16).  Control  law  design  consists  of  finding 
the  error  function  w(e)  which  makes  (C.18)  globally  asymptotically  stable,1 
i.e., 

lim  e(t)  =  0. 

t—>  OO 

In  the  case  of  perfect  nonlinear  compensation  (z(-)  =  0),  the  simplest  choice 
of  the  control  action  is  the  linear  one 


w(e)  =  ~KP{xd  -  x)  -  KD(xd  -  x)  (C.19) 

=  [-KP  -KD\e, 


1  Global  asymptotic  stability  is  invoked  to  remark  that  the  equilibrium  state  is 
asymptotically  stable  for  any  perturbation. 


596  C  Feedback  Control 


where  asymptotic  stability  of  the  error  equation  is  ensured  by  choosing  positive 
definite  matrices  Kp  and  Kp.  The  error  transient  behaviour  is  determined 
by  the  eigenvalues  of  the  matrix 


A  = 


O 

KP 


I 

-kd 


(C.20) 


characterizing  the  error  dynamics 


e  =  Ae.  (C.21) 

If  compensation  is  imperfect,  then  z(-)  cannot  be  neglected  and  the  error 
equation  in  (C.18)  takes  on  the  general  form 

e  =  /(e).  (C.22) 

It  may  be  worth  choosing  the  control  law  w(e)  as  the  sum  of  a  nonlinear  term 
and  a  linear  term  of  the  kind  in  (C.19);  in  this  case,  the  error  equation  can 
be  written  as 

e  =  Ae  +  k(e),  (C.23) 

where  A  is  given  by  (C.20)  and  fc(e)  is  available  to  make  the  system  globally 
asymptotically  stable.  The  equations  in  (C.22),  (C.23)  express  nonlinear  dif¬ 
ferential  equations  of  the  error.  To  test  for  stability  and  obtain  advise  on  the 
choice  of  suitable  control  actions,  one  may  resort  to  Lyapunov  direct  method 
illustrated  below. 


C.3  Lyapunov  Direct  Method 

The  philosophy  of  the  Lyapunov  direct  method  is  the  same  as  that  of  most 
methods  used  in  control  engineering  to  study  stability,  namely,  testing  for 
stability  without  solving  the  differential  equations  describing  the  dynamic 
system. 

This  method  can  be  presented  in  short  on  the  basis  of  the  following  rea¬ 
soning.  If  it  is  possible  to  associate  an  energy-based  description  with  a  (linear 
or  nonlinear)  autonomous  dynamic  system  and,  for  each  system  state  with  the 
exception  of  the  equilibrium  state,  the  time  rate  of  such  energy  is  negative, 
then  energy  decreases  along  any  system  trajectory  until  it  attains  its  mini¬ 
mum  at  the  equilibrium  state;  this  argument  justifies  an  intuitive  concept  of 
stability. 

With  reference  to  (C.22),  by  setting  /( 0)  =  0,  the  equilibrium  state  is 
e  =  0.  A  scalar  function  V(e)  of  the  system  state,  continuous  together  with 
its  first  derivative,  is  defined  a  Lyapunov  function  if  the  following  properties 
hold: 


V(e)  >  0 


Ve  ±  0 
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V(e)  =  0  e  =  0 

V(e)  <0  Ve  ±  0 

V(e)  — »  oo  ||e||  — *  oo. 

The  existence  of  such  a  function  ensures  global  asymptotic  stability  of  the  equi¬ 
librium  e  =  0.  In  practice,  the  equilibrium  e  =  0  is  globally  asymptotically 
stable  if  a  positive  definite,  radially  unbounded  function  V  (e)  is  found  so  that 
its  time  derivative  along  the  system  trajectories  is  negative  definite. 

If  positive  definiteness  of  V (e)  is  realized  by  the  adoption  of  a  quadratic 
form,  i.e., 

V(e)  =  eTQe  (C.24) 

with  Q  a  symmetric  positive  definite  matrix,  then  in  view  of  (C.22)  it  follows 

V  (e)  =  2  eTQf{e).  (C.25) 

If  /(e)  is  so  as  to  render  the  function  V(e)  negative  definite,  the  function 

V(e)  is  a  Lyapunov  function,  since  the  choice  (C.24)  allows  system  global 

asymptotic  stability  to  be  proved.  If  V(e)  in  (C.25)  is  not  negative  definite 
for  the  given  V (e) ,  nothing  can  be  inferred  on  the  stability  of  the  system, 
since  the  Lyapunov  method  gives  only  a  sufficient  condition.  In  such  cases 
one  should  resort  to  different  choices  of  V(e)  in  order  to  find,  if  possible,  a 
negative  definite  V(e). 

In  the  case  when  the  property  of  negative  definiteness  does  not  hold,  but 
V (e)  is  only  negative  semi-definite 

V(e)  <  0, 

global  asymptotic  stability  of  the  equilibrium  state  is  ensured  if  the  only  sys¬ 
tem  trajectory  for  which  V (e)  is  identically  null  (V (e)  =  0)  is  the  equilibrium 
trajectory  e  =  0  (a  consequence  of  La  Salle  theorem). 

Finally,  consider  the  stability  problem  of  the  nonlinear  system  in  the 
form  (C.23);  under  the  assumption  that  fc(0)  =  0,  it  is  easy  to  verify  that 
e  =  0  is  an  equilibrium  state  for  the  system.  The  choice  of  a  Lyapunov  func¬ 
tion  candidate  as  in  (C.24)  leads  to  the  following  expression  for  its  derivative: 


V(e)  =  er(AI  Q  +  QA)e  +  2 eTQk(e). 

(C.26) 

By  setting 

AT  Q  +  QA  =  — P, 

(C.27) 

the  expression  in  (C.26)  becomes 

V  (e)  =  eT  Pe  +  2  eTQk(e) . 

(C.28) 

The  matrix  equation  in  (C.27)  is  said  to  be  a  Lyapunov  equation;  for  any 
choice  of  a  symmetric  positive  definite  matrix  P,  the  solution  matrix  Q  exists 
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and  is  symmetric  positive  definite  if  and  only  if  the  eigenvalues  of  A  have 
all  negative  real  parts.  Since  matrix  A  in  (C.20)  verifies  such  condition,  it 
is  always  possible  to  assign  a  positive  definite  matrix  P  and  find  a  positive 
definite  matrix  solution  Q  to  (C.27).  It  follows  that  the  first  term  on  the 
right-hand  side  of  (C.28)  is  negative  definite  and  the  stability  problem  is 
reduced  to  searching  a  control  law  so  that  k(e)  renders  the  total  V (e)  negative 
(semi-)  definite. 

It  should  be  underlined  that  La  Salle  theorem  does  not  hold  for  time- 
varying  systems  (also  termed  non- autonomous)  in  the  form 

e  =  /(e,t). 

In  this  case,  a  conceptually  analogous  result  which  might  be  useful  is  the 
following,  typically  referred  to  as  Barbalat  lemma  —  of  which  it  is  indeed  a 
consequence.  Given  a  scalar  function  V (e,  t)  so  that 

1.  V (e,  t)  is  lower  bounded 

2.  V(e,t)  <  0 

3.  V (e,  t)  is  uniformly  continuous 

then  it  is  lim^oo  V(e,  t)  =  0.  Conditions  1  and  2  imply  that  V(e,t)  has  a 
bounded  limit  for  t  — »  oo.  Since  it  is  not  easy  to  verify  the  property  of  uniform 
continuity  from  the  definition,  Condition  3  is  usually  replaced  by 

3’.  V(e,t)  is  bounded 

which  is  sufficient  to  guarantee  validity  of  Condition  3.  Barbalat  lemma  can 
obviously  be  used  for  time-invariant  (autonomous)  dynamic  systems  as  an 
alternative  to  La  Salle  theorem,  with  respect  to  which  some  conditions  are 
relaxed;  in  particular,  V (e)  needs  not  necessarily  be  positive  definite. 
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control  of  these  systems  see  [82,  171].  For  the  analysis  of  nonlinear  systems 
see  [109].  Control  of  nonlinear  mechanical  systems  is  dealt  with  in  [215]. 


D 


Differential  Geometry 


The  analysis  of  mechanical  systems  subject  to  nonholonomic  constraints,  such 
as  wheeled  mobile  robots,  requires  some  basic  concepts  of  differential  geometry 
and  nonlinear  controllability  theory,  that  are  briefly  recalled  in  this  appendix. 


D.l  Vector  Fields  and  Lie  Brackets 

For  simplicity,  the  case  of  vectors  x  £  IR"  is  considered.  The  tangent  space 
at  x  (intuitively,  the  space  of  velocities  of  trajectories  passing  through  x)  is 
hence  denoted  by  Tx  (IFi.n ) .  The  presented  notions  are  however  valid  in  the 
more  general  case  in  which  a  differentiable  manifold  (i.e.,  a  space  that  is 
locally  diffeomorphic  to  IR")  is  considered  in  place  of  a  Euclidean  space. 

A  vector  field  g  :  IR"  i— >  Tx(Ein)  is  a  mapping  that  assigns  to  each  point 
x  £  IR"  a  tangent  vector  g(x)  £  Tx(IRn).  In  the  following  it  is  always  assumed 
that  vector  fields  are  smooth ,  i.e.,  such  that  the  associated  mappings  are  of 
class  C°°. 

If  the  vector  field  g(x)  is  used  to  define  a  differential  equation  as  in 

x  =  g(x),  (D.l) 

the  flow  (ft  (; x )  of  g  is  the  mapping  that  associates  to  each  point  x  the  value 
at  time  t  of  the  solution  of  (D.l)  evolving  from  x  at  time  0,  or 

jt4^{x)=g{4>^{x)).  (D.2) 

The  family  of  mappings  {<ft}  is  a  one-parameter  (i.e.,  t)  group  under  the 
composition  operator 

K  °  4>l  =  4>?l+t2- 

For  example,  for  time-invariant  linear  systems  it  is  g(x)  =  Ax  and  the  flow 
is  the  linear  operator  <fff  =  e J^t. 
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Fig.  D.l.  The  net  displacement  of  system  (D.4)  under  the  input  sequence  (D.5)  is 
directed  as  the  Lie  bracket  of  the  two  vector  fields  g1  and  g2 


Given  two  vector  fields  g1  and  g2,  the  composition  of  their  flows  is  non- 
commutative  in  general: 

The  vector  field  [gf1,S'2]  defined  as 

[9i,92\(x)=d^g1{x)-d^g2(x)  (D.3) 

is  called  Lie  bracket  of  g±  and  g2 .  The  two  vector  field  g±  and  g2  commute  if 
[9i,92]  =  0. 

The  Lie  bracket  operation  has  an  interesting  interpretation.  Consider  the 
driftless  dynamic  system 

x  =  9i(*)«i  +  g-2(x)u  2  (D.4) 


associated  with  the  vector  fields  gx  and  g2.  If  the  inputs  U\  and  u2  are  never 
active  simultaneously,  the  solution  of  the  differential  equation  (D.4)  can  be 
obtained  by  composing  the  flows  of  g1  and  g2.  In  particular,  consider  the 
following  input  sequence: 


!u\(t)  =  +l,u2(t)  =  0  i€[0,e) 

ui(t)  =  0, u2(t)  =  +1  f  6  [e, 2 e) 
ui(t)  =  —  l,U2(t)  =  0  f  e  [2e, 3e) 
ui(t)  =  0 ,  u2(t)  =  — 1  t  G  [3e,  4s), 


where  e  is  an  infinitesimal  time  interval.  The  solution  of  (D.4)  at  time  t  =  4e 
can  be  obtained  by  following  first  the  flow  of  <jq,  then  of  g2,  then  of  —  glt  and 
finally  of  —  g2  (see  Fig.  D.l).  By  computing  x(s)  through  a  series  expansion 
at  Xq  =  x(0)  along  g1,  then  x(2s )  as  a  series  expansion  at  x(s)  along  g2 ,  and 
so  on,  one  obtains 

*(4e)  =  fc92  o  4>e  9l  o  </>f2  o  <j)9l(x0) 

=  x0  +  s2  gx(x 0)  -  ^-g2(x 0)j  +  0(e3). 
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If  g1  and  g2  commute,  the  net  displacement  resulting  from  the  input  se¬ 
quence  (D.5)  is  zero. 

The  above  expression  shows  that,  at  each  point  x ,  infinitesimal  motion 
of  the  driftless  system  (D.4)  is  possible  not  only  in  the  directions  belonging 
to  the  linear  span  of  g\(x)  and  g2(x),  but  also  in  the  direction  of  their  Lie 
bracket  [g1,g2\(x).  It  can  be  proven  that  more  complicated  input  sequences 
can  be  used  to  generate  motion  in  the  direction  of  higher-order  Lie  brackets, 
such  as  \gu  \g1,g2]}- 

Similar  constructive  procedures  can  be  given  for  systems  with  a  drift 1 
vector  field,  such  as  the  following: 

x  =  f(x)  +  g1(x)u1+g2(x)u2.  (D.6) 

Using  appropriate  input  sequences,  it  is  possible  to  generate  motion  in  the 
direction  of  Lie  brackets  involving  the  vector  field  /  as  well  as  gj ,  j  =  1,2. 


Example  D.l 

For  a  single-input  linear  system 


x  =  A  x  +  b  u, 

the  drift  and  input  vector  fields  are  f{x)  =  Ax  and  g(x)  =  b,  respectively.  The 
following  Lie  brackets: 


-If,  a]  =  Ab 

[f,[f,g]]  =  A2b 
-[f,[f,lf,g}}]  =  A3b 


represent  well-known  directions  in  which  it  is  possible  to  move  the  system. 


The  Lie  derivative  of  the  scalar  function  a  :  IR71  i— >  1R  along  vector  field  g 
is  defined  as 

Lga(x)  =  ^g{x).  (D.7) 

The  following  properties  of  Lie  brackets  are  useful  in  computation: 

[/,  g]  =  —[<7,  /]  (skew-symmetry) 

[/,  [g,  h}}  +  [h,  [/,  g}}  +  [g,  [h,  /]]  =  0  (Jacobi  identity) 

[af,  j3g\  =  a(3 [/,  g\  +  a(L  j/3)g  -  /3(Lga)f  (chain  rule) 


1  This  term  emphasizes  how  the  presence  of  /  will  in  general  force  the  system  to 
move  (x  0)  even  in  the  absence  of  inputs. 
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with  a,  (3: IR"  i— >  IR.  The  vector  space  V(1R™)  of  smooth  vector  fields  on  IRra, 
equipped  with  the  Lie  bracket  operation,  is  a  Lie  algebra. 

The  distribution  A  associated  with  the  m  vector  fields  {gq, . . .  ,gm }  is  the 
mapping  that  assigns  to  each  point  x  G  IR"  the  subspace  of  TX(]R")  defined 
as 

A(x)  =span {g^x),. . .  ,gm(x)}.  (D.8) 

Often,  a  shorthand  notation  is  used: 

A  =  span{g1, . . . ,  gm}. 

The  distribution  A  is  nonsingular  if  dimZ\(a;)  =  r,  with  r  constant  for  all 
x.  In  this  case,  r  is  called  the  dimension  of  the  distribution.  Moreover,  A  is 
called  involutive  if  it  is  closed  under  the  Lie  bracket  operation: 

\9i,9j\€A  V <7i;  <7j  €  A. 


The  involutive  closure  A  of  a  distribution  A  is  its  closure  under  the  Lie  bracket 
operation.  Hence,  A  is  involutive  if  and  only  if  A  =  A.  Note  that  the  distri¬ 
bution  A  =  spanjg}  associated  with  a  single  vector  field  is  always  involutive, 
because  [g,g](x)  =  0. 


Example  D.2 

The  distribution 


zi  =  span{g1,g2}  =  span  • 


f 

"  COS  X3  " 

/°\l 

span  < 

sin  X3 

’  0 

l 

0 

Vi/J 

is  nonsingular  and  has  dimension  2.  It  is  not  involutive,  because  the  Lie  bracket 


[91,92  ](*) 


sinx3 
—cos  X3 
0 


is  always  linearly  independent  of  gi(x)  and  g2(x).  Its  involutive  closure  is  therefore 

zi  =  spanjg^Sa,  [g1.g2\}. 


D  Differential  Geometry  603 


D.2  Nonlinear  Controllability 

Consider  a  nonlinear  dynamic  system  of  the  form 

m 

x  =  f(x)  +'^2gj(x)uj,  (D.9) 

5=1 

that  is  called  affine  in  the  inputs  Uj.  The  state  x  takes  values  in  IR”,  while 
each  component  Uj  of  the  control  input  u  G  IR"1  takes  values  in  the  class  U 
of  piece  wise-const  ant  functions. 

Denote  by  x(t,  0,  x0,  u)  the  solution  of  (D.9)  at  time  t  >  0,  corresponding 
to  an  input  u:  [0,t]  — >  U  and  an  initial  condition  x(0)  =  xo-  Such  a  solution 
exists  and  is  unique  provided  that  the  drift  vector  field  /  and  the  input  vector 
fields  gfj  are  of  class  C°°.  System  (D.9)  is  said  to  be  controllable  if,  for  any 
choice  of  aq ,  X2  in  IR™,  there  exists  a  time  instant  T  and  an  input  u:  [0,  T\  — ►  U 
such  that  x(T,  0,  Xi,  u)  =  x2. 

The  accessibility  algebra  A  of  system  (D.9)  is  the  smallest  subalgebra  of 
V(1R")  that  contains  gm.  By  definition,  all  the  Lie  brackets  that  can 

be  generated  using  these  vector  fields  belong  to  A.  The  accessibility  distribu¬ 
tion  Ay i  of  system  (D.9)  is  defined  as 

Ay\  =  span{u|u  G  .4}.  (D.10) 

In  other  words,  Ay i  is  the  involutive  closure  of  A  =  span{/,glt . . .  ,gm}. 

The  computation  of  Ay\  may  be  organized  as  an  iterative  procedure 

A_4  =  span{u|u  G  >  1}  , 


with 


Ai  =  A  =  span{/ ,  gl5 . . . ,  gm} 

A i  =  Aj_i  +  span{[g,  v]\g  G  Ai,  v  €  i},  i  >  2. 

This  procedure  stops  after  k  steps,  where  k  is  the  smallest  integer  such  that 
Ak+  i  =  Ak  =  Ay\.  This  number  is  called  the  nonholonomy  degree  of  the 
system  and  is  related  to  the  ‘level’  of  Lie  brackets  that  must  be  included  in 
A_4 .  Since  dim  A_a  <  n,  it  is  k  <  n  —  m  necessarily. 

If  system  (D.9)  is  driftless 


v  =  '529i(x)ui,  (D.ll) 

i= 1 

the  accessibility  distribution  Ay {  associated  with  vector  fields  gm  char¬ 

acterizes  its  controllability.  In  particular,  system  (D.ll)  is  controllable  if  and 
only  if  the  following  accessibility  rank  condition  holds: 


dim  Ay\(x)  =  n. 


(D.12) 
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Note  that  for  driftless  systems  the  iterative  procedure  for  building  A_4  starts 
with  Ai  =  A  =  span{gl5 . . . , gm},  and  therefore  k  <  n  —  m  +  1. 

For  systems  in  the  general  form  (D.9),  condition  (D.12)  is  only  necessary 
for  controllability.  There  are,  however,  two  notable  exceptions: 

•  If  system  (D.ll)  is  controllable,  the  system  with  drift  obtained  by  per¬ 
forming  a  dynamic  extension  of  (D.ll) 

m 

x  =  ^jgi{x)vi  (D.13) 

2=1 

Vi  =  Ui,  i  =  1, . . .  ,m,  (D.14) 

i.e. ,  by  adding  an  integrator  on  each  input  channel,  is  also  controllable. 

•  For  a  linear  system 


m 

x  =  Ax  +  bjUj  =  Ax  +  Bu 

3= 1 


(D.12)  becomes 


e{[B  AB  A2B  ...  An~1B  ])  =  n,  (D.15) 

i.e.,  the  well-known  necessary  and  sufficient  condition  for  controllability 
due  to  Kalman. 
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The  concepts  briefly  recalled  in  this  appendix  can  be  studied  in  detail  in 
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Graph  Search  Algorithms 


This  appendix  summarizes  some  basic  concepts  on  algorithm  complexity  and 
graph  search  techniques  that  are  useful  in  the  study  of  motion  planning. 


E.l  Complexity 

A  major  criterion  for  assessing  the  efficiency  of  an  algorithm  A  is  its  running 
time ,  i.e.,  the  time  needed  for  executing  the  algorithm  in  a  computational 
model  capturing  the  most  relevant  characteristics  of  an  actual  elaboration 
system.  In  practice,  one  is  interested  in  estimating  the  running  time  as  a 
function  of  a  single  parameter  n  characterizing  the  size  of  the  input  within  a 
specific  class  of  instances  of  the  problem.  In  motion  planning,  this  parameter 
may  be  the  dimension  of  the  configuration  space,  or  the  number  of  vertices  of 
the  free  configuration  space  (if  it  is  a  polygonal  subset) . 

In  worst-case  analysis,  t{n )  denotes  the  maximum  running  time  of  A  in 
correspondence  of  input  instances  of  size  n.  Other  kinds  of  analyses  (e.g., 
average-case)  are  possible  but  they  are  less  critical  or  general,  requiring  a 
statistical  knowledge  of  the  input  distribution  that  may  not  be  available. 

The  exact  functional  expression  of  t(n)  depends  on  the  implementation  of 
the  algorithm,  and  is  of  little  practical  interest  because  the  running  time  in 
the  adopted  computational  model  is  only  an  approximation  of  the  actual  one. 
More  significant  is  the  asymptotic  behaviour  of  t(n),  i.e.,  the  rate  of  growth 
of  t(n)  with  n.  Denote  by  the  set  of  real  functions  g(n)  such  that 

ci/(n)  <  g(n)  <  c2/(n)  Vn>  n0, 

with  ci,  Ci  and  no  positive  constants.  If  the  worst-case  running  time  of  A  is 
0(f(n)),  i.e.,  if  t(n)  €  the  time  complexity  of  A  is  said  to  be  0(f(ri)). 

A  very  important  class  is  represented  by  algorithms  whose  worst-case  run¬ 
ning  time  is  asymptotically  polynomial  in  the  size  of  the  input.  In  particular, 
if  t[n)  G  0(np),  for  some  p  >  0,  the  algorithm  is  said  to  have  polynomial  time 
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complexity.  If  the  asymptotic  behaviour  of  the  worst-case  running  time  is  not 
polynomial,  the  time  complexity  of  the  algorithm  is  exponential.  Note  that 
here  ‘exponential’  actually  means  ‘not  bounded  by  any  polynomial  function’. 

The  asymptotic  behaviour  of  an  algorithm  with  exponential  time  complex¬ 
ity  is  such  that  in  the  worst  case  it  can  only  be  applied  to  problems  of  ‘small’ 
size.  However,  there  exist  algorithms  of  exponential  complexity  that  are  very 
efficient  on  average,  i.e. ,  for  the  most  frequent  classes  of  input.  A  well  known 
example  is  the  simplex  algorithm  for  solving  linear  programming  problems. 
Similarly,  there  are  algorithms  with  polynomial  time  complexity  which  are 
inefficient  in  practice  because  c\,  C2  or  p  are  ‘large’. 

The  above  concepts  can  be  extended  to  inputs  whose  size  is  characterized 
by  more  than  one  parameter,  or  to  performance  criteria  different  from  running 
time.  For  example,  the  memory  space  required  by  an  algorithm  is  another 
important  measure.  The  space  complexity  of  an  algorithm  is  said  to  be  0(/(n)) 
if  the  memory  space  required  for  its  execution  is  a  function  in  0(/(n)). 


E.2  Breadth- first  and  Depth-first  Search 

Let  G  =  (N,  A)  be  a  graph  consisting  of  a  set  N  of  nodes  and  a  set  A  of  arcs, 
with  cardinality  n  and  a  respectively.  It  is  assumed  that  G  is  represented 
by  an  adjacency  list:  to  each  node  N  is  associated  a  list  of  nodes  that  are 
connected  to  N,:  by  an  arc.  Consider  the  problem  of  searching  G  to  find  a  path 
from  a  start  node  Ns  to  a  goal  node  Ng.  The  simplest  graph  search  strategies 
are  breadth-jirst  search  (BFS)  and  depth- first  search  (DFS).  These  are  briefly 
described  in  the  following  with  reference  to  an  iterative  implementation. 

Breadth-first  search  makes  use  of  a  queue  —  i.e.,  a  FIFO  (First  In  First 
Out)  data  structure  —  -  of  nodes  called  OPEN.  Initially,  OPEN  contains  only 
the  start  node  Ns,  which  is  marked  visited.  All  the  other  nodes  in  G  are  marked 
unvisited.  At  each  iteration  the  first  node  in  OPEN  is  extracted,  and  all  its 
unvisited  adjacent  nodes  are  marked  visited  and  inserted  in  OPEN.  The  search 
terminates  when  either  Ng  is  inserted  in  OPEN  or  OPEN  is  empty  (failure). 
During  the  search,  the  algorithm  maintains  the  BFS  tree,  which  contains  only 
those  arcs  that  have  led  to  discovering  unvisited  nodes.  This  tree  contains  one 
and  only  one  path  connecting  the  start  node  to  each  visited  node,  and  hence 
also  a  solution  path  from  Ns  to  Ng,  if  it  exists. 

In  depth-first  search,  OPEN  is  a  stack,  i.e.,  a  LIFO  (Last  In  First  Out) 
data  structure.  Like  in  the  breadth-first  case,  it  contains  initially  only  the 
start  node  Ns  marked  visited.  When  a  node  Nj  is  inserted  in  OPEN,  the  node 
Nj  which  has  determined  its  insertion  is  memorized.  At  each  iteration,  the 
first  node  in  OPEN  is  extracted.  If  it  is  unvisited,  it  is  marked  visited  and 
the  arc  connecting  AT,;  to  Nj  is  inserted  in  the  DFS  tree.  All  unvisited  nodes 
that  are  adjacent  to  Nj  are  inserted  in  OPEN.  The  search  terminates  when 
either  Ng  is  inserted  in  OPEN  or  OPEN  is  empty  (failure).  Like  in  the  BFS, 
the  DFS  tree  contains  the  solution  path  from  Ns  to  Ng,  if  it  exists. 
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Both  breadth-first  and  depth- first  search  have  time  complexity  O(a).  Note 
that  BFS  and  DFS  are  actually  traversal  strategies,  because  they  do  not  use 
any  information  about  the  goal  node;  the  graph  is  simply  traversed  until  Ng 
is  marked  visited.  Both  the  algorithms  are  complete,  i.e.,  they  find  a  solution 
path  if  it  exists  and  report  failure  otherwise. 


E.3  A *  Algorithm 

In  many  applications,  the  arcs  of  G  are  labelled  with  positive  numbers  called 
weights.  As  a  consequence,  one  may  define  the  cost  of  a  path  on  G  as  the 
sum  of  the  weights  of  its  arcs.  Consider  the  problem  of  connecting  Ns  to 
Ng  on  G  through  a  path  of  minimum  cost,  simply  called  minimum  path.  In 
motion  planning  problems,  for  example,  the  nodes  generally  represent  points 
in  configuration  space,  and  it  is  then  natural  to  define  the  weight  of  an  arc 
as  the  length  of  the  path  that  it  represents.  The  minimum  path  is  obviously 
interesting  because  it  is  the  shortest  among  those  joining  Ns  to  Ng  on  G. 

A  widely  used  strategy  for  determining  the  minimum  path  on  a  graph  is 
the  A*  algorithm.  A*  visits  the  nodes  of  G  iteratively  starting  from  Ns,  storing 
only  the  current  minimum  paths  from  Ns  to  the  visited  nodes  in  a  tree  T. 
The  algorithm  employs  a  cost  function  /(A* )  for  each  node  Nt  visited  during 
the  search.  This  function,  which  is  an  estimate  of  the  cost  of  the  minimum 
path  that  connects  Ns  to  Ng  passing  through  A*,  is  computed  as 


f(Ni)  =  g(Ni)  +  h(Ni), 


where  g(Aj)  is  the  cost  of  the  path  from  Ns  to  A,  as  stored  in  the  current 
tree  T,  and  /i(A,; )  is  a  heuristic  estimate  of  the  cost  h*(Ni)  of  the  minimum 
path  between  A,;  and  Ng.  While  the  value  of  <7 (A* )  is  uniquely  determined  by 
the  search,  any  choice  of  h(-)  such  that 


VA,;  e  A  :  0  <  h(Ni)  <  /i*(A4)  (E.l) 

is  admissible.  Condition  (E.l)  means  that  h(-)  must  not  ‘overestimate’  the 
cost  of  the  minimum  path  from  A,  to  Ng. 

In  the  following,  a  pseudocode  description  of  A*  is  given.  For  its  under¬ 
standing,  some  preliminary  remarks  are  needed: 

•  all  the  nodes  are  initially  unvisited,  except  Ns  which  is  visited', 

•  at  the  beginning,  T  contains  only  Ns; 

•  OPEN  is  a  list  of  nodes  that  initially  contains  only  As; 

•  A best  is  the  node  in  OPEN  with  the  minimum  value  of  /  (in  particular,  it 
is  the  first  node  if  OPEN  is  sorted  by  increasing  values  of  /); 

•  ADJ(Aj)  is  the  adjacency  list  of  A,; 

•  c(Aj,  Nj)  is  the  weight  of  the  arc  connecting  Aj  to  Nj. 
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A*  algorithm 

1  repeat 

2  find  and  extract  TVbest  from  OPEN 

3  if  TVbest  =  Ng  then  exit 

4  for  each  node  TV,  in  ADJ(TVbest)  do 

5  if  Ni  is  unvisited  then 

6  add  JVj  to  T  with  a  pointer  toward  TVbest 

7  insert  TV,  in  OPEN;  mark  TV,  visited 

8  else  if  .g(TVbest)  +  c(TVbest,  N)  <  g{Nf)  then 

9  redirect  the  pointer  of  Ni  in  T  toward  TVbest 

10  if  Ni  is  not  in  OPEN  then 

10  insert  TV,  in  OPEN 

10  else  update  /(TV,) 

10  end  if 

11  end  if 

12  until  OPEN  is  empty 

Under  condition  (E.l),  the  A*  algorithm  is  complete.  In  particular,  if  the 
algorithm  terminates  with  an  empty  OPEN,  there  exists  no  path  in  G  from 
TV,  to  Ng  (failure) ;  otherwise,  the  tree  T  contains  the  minimum  path  from  TV, 
to  Ng,  which  can  be  reconstructed  by  backtracking  from  Ng  to  TVS. 

The  A *  algorithm  with  the  particular  (admissible)  choice  Ti(TVj)  =  0,  for 
each  node  TVj,  is  equivalent  to  the  Dijkstra  algorithm.  If  the  nodes  in  G  rep¬ 
resent  points  in  a  Euclidean  space,  an  admissible  heuristic  is  the  Euclidean 
distance  between  TV  and  Ng.  In  fact,  the  length  of  the  minimum  path  between 
TV,  and  Ng  is  bounded  below  by  the  Euclidean  distance. 

The  extraction  of  a  node  from  OPEN  and  the  visit  of  its  adjacent  nodes 
is  called  node  expansion.  Given  two  admissible  heuristic  functions  hi  and  /12 
such  that  Ti2(TVi)  >  hi  (A/),  for  each  node  TV,  in  G,  it  is  possible  to  prove 
that  each  node  in  G  expanded  by  A*  using  ft2  is  also  expanded  using  hi.  This 
means  that  A *  equipped  with  the  heuristic  Ti2  is  at  least  as  efficient  as  A* 
equipped  with  the  heuristic  hi’,  T*2  is  said  to  be  more  informed  than  hi. 

The  A*  algorithm  can  be  implemented  with  time  complexity  O(alogn). 
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determinant,  566 
diagonal,  564 
eigenvalues,  573 
eigenvectors,  573 
essential,  434 

homogeneous  transformation,  56 

idempotent,  568 

identity,  564 

inertia,  254 

interaction,  424 

inverse,  567 

Jacobian,  569 
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second-order  kinematic  model,  489 
unit  quaternion,  54,  140 
unit  vector 
approach,  59 
binormal,  181 
control,  337 
normal,  59,  181 
sliding,  59 
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